// Copyright Louis Dionne 2013-2017
// 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_HANA_TEST_AUTO_CARTESIAN_PRODUCT_HPP
#define BOOST_HANA_TEST_AUTO_CARTESIAN_PRODUCT_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/cartesian_product.hpp>
#include <boost/hana/equal.hpp>

#include <laws/base.hpp>
#include "test_case.hpp"

TestCase test_cartesian_product{[]{
    namespace hana = boost::hana;
    using hana::test::ct_eq;

    // 0 lists
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE()),
        MAKE_TUPLE()
    ));

    // 1 list
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE()
        )),
        MAKE_TUPLE()
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{})
        )
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{}, ct_eq<1>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{}),
            MAKE_TUPLE(ct_eq<1>{})
        )
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{}, ct_eq<1>{}, ct_eq<2>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{}),
            MAKE_TUPLE(ct_eq<1>{}),
            MAKE_TUPLE(ct_eq<2>{})
        )
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{}, ct_eq<1>{}, ct_eq<2>{}, ct_eq<3>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<0>{}),
            MAKE_TUPLE(ct_eq<1>{}),
            MAKE_TUPLE(ct_eq<2>{}),
            MAKE_TUPLE(ct_eq<3>{})
        )
    ));

    // 2 lists
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(),
            MAKE_TUPLE()
        )),
        MAKE_TUPLE()
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}),
            MAKE_TUPLE()
        )),
        MAKE_TUPLE()
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}),
            MAKE_TUPLE(ct_eq<10>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{})
        )
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<01>{}),
            MAKE_TUPLE(ct_eq<10>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}),
            MAKE_TUPLE(ct_eq<01>{}, ct_eq<10>{})
        )
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}),
            MAKE_TUPLE(ct_eq<10>{}, ct_eq<11>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}),
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<11>{})
        )
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<01>{}),
            MAKE_TUPLE(ct_eq<10>{}, ct_eq<11>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}),
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<11>{}),
            MAKE_TUPLE(ct_eq<01>{}, ct_eq<10>{}),
            MAKE_TUPLE(ct_eq<01>{}, ct_eq<11>{})
        )
    ));

    // misc
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}),
            MAKE_TUPLE(ct_eq<10>{}),
            MAKE_TUPLE(ct_eq<20>{}),
            MAKE_TUPLE(ct_eq<30>{}, ct_eq<31>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}, ct_eq<20>{}, ct_eq<30>{}),
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}, ct_eq<20>{}, ct_eq<31>{})
        )
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}),
            MAKE_TUPLE(ct_eq<10>{}),
            MAKE_TUPLE(ct_eq<20>{}, ct_eq<21>{}),
            MAKE_TUPLE(ct_eq<30>{}, ct_eq<31>{})
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}, ct_eq<20>{}, ct_eq<30>{}),
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}, ct_eq<20>{}, ct_eq<31>{}),
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}, ct_eq<21>{}, ct_eq<30>{}),
            MAKE_TUPLE(ct_eq<00>{}, ct_eq<10>{}, ct_eq<21>{}, ct_eq<31>{})
        )
    ));

    // cartesian_product in a constexpr context
#ifndef MAKE_TUPLE_NO_CONSTEXPR
    static_assert(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(1),
            MAKE_TUPLE('a', 'b')
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(1, 'a'),
            MAKE_TUPLE(1, 'b')
        )
    ), "");

    static_assert(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(1, 2),
            MAKE_TUPLE('a')
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(1, 'a'),
            MAKE_TUPLE(2, 'a')
        )
    ), "");

    static_assert(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(1, 2),
            MAKE_TUPLE('a', 'b')
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(1, 'a'),
            MAKE_TUPLE(1, 'b'),
            MAKE_TUPLE(2, 'a'),
            MAKE_TUPLE(2, 'b')
        )
    ), "");

    static_assert(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(1),
            MAKE_TUPLE('a'),
            MAKE_TUPLE(1.f),
            MAKE_TUPLE(1l, 2l)
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(1, 'a', 1.f, 1l),
            MAKE_TUPLE(1, 'a', 1.f, 2l)
        )
    ), "");

    static_assert(hana::equal(
        hana::cartesian_product(MAKE_TUPLE(
            MAKE_TUPLE(1),
            MAKE_TUPLE('a'),
            MAKE_TUPLE(1.f),
            MAKE_TUPLE(1l, 2l),
            MAKE_TUPLE(nullptr)
        )),
        MAKE_TUPLE(
            MAKE_TUPLE(1, 'a', 1.f, 1l, nullptr),
            MAKE_TUPLE(1, 'a', 1.f, 2l, nullptr)
        )
    ), "");
#endif
}};

#endif // !BOOST_HANA_TEST_AUTO_CARTESIAN_PRODUCT_HPP


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>

#include <boost/compute/core.hpp>

namespace compute = boost::compute;

// this example demonstrates how to use the Boost.Compute classes to
// setup and run a simple vector addition kernel on the GPU
int main()
{
    // get the default device
    compute::device device = compute::system::default_device();

    // create a context for the device
    compute::context context(device);

    // setup input arrays
    float a[] = { 1, 2, 3, 4 };
    float b[] = { 5, 6, 7, 8 };

    // make space for the output
    float c[] = { 0, 0, 0, 0 };

    // create memory buffers for the input and output
    compute::buffer buffer_a(context, 4 * sizeof(float));
    compute::buffer buffer_b(context, 4 * sizeof(float));
    compute::buffer buffer_c(context, 4 * sizeof(float));

    // source code for the add kernel
    const char source[] =
        "__kernel void add(__global const float *a,"
        "                  __global const float *b,"
        "                  __global float *c)"
        "{"
        "    const uint i = get_global_id(0);"
        "    c[i] = a[i] + b[i];"
        "}";

    // create the program with the source
    compute::program program =
        compute::program::create_with_source(source, context);

    // compile the program
    program.build();

    // create the kernel
    compute::kernel kernel(program, "add");

    // set the kernel arguments
    kernel.set_arg(0, buffer_a);
    kernel.set_arg(1, buffer_b);
    kernel.set_arg(2, buffer_c);

    // create a command queue
    compute::command_queue queue(context, device);

    // write the data from 'a' and 'b' to the device
    queue.enqueue_write_buffer(buffer_a, 0, 4 * sizeof(float), a);
    queue.enqueue_write_buffer(buffer_b, 0, 4 * sizeof(float), b);

    // run the add kernel
    queue.enqueue_1d_range_kernel(kernel, 0, 4, 0);

    // transfer results back to the host array 'c'
    queue.enqueue_read_buffer(buffer_c, 0, 4 * sizeof(float), c);

    // print out results in 'c'
    std::cout << "c: [" << c[0] << ", "
                        << c[1] << ", "
                        << c[2] << ", "
                        << c[3] << "]" << std::endl;

    return 0;
}


/*
 * test_SGN.cpp
 *
 *  Created on: 2019年4月3日
 *      Author: fasiondog
 */

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

#include <fstream>
#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/crt/SGN.h>
#include <hikyuu/indicator/crt/KDATA.h>
#include <hikyuu/indicator/crt/PRICELIST.h>

using namespace hku;

/**
 * @defgroup test_indicator_SGN test_indicator_SGN
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_SGN ) {
    Indicator result;

    PriceList a;
    a.push_back(-10);
    a.push_back(0);
    a.push_back(10);

    Indicator data = PRICELIST(a);

    result = SGN(data);
    BOOST_CHECK(result.name() == "SGN");
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result.size() == 3);
    BOOST_CHECK(result[0] == -1);
    BOOST_CHECK(result[1] == 0);
    BOOST_CHECK(result[2] == 1);

    result = SGN(-1);
    BOOST_CHECK(result.size() == 1);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result[0] == -1);

    result = SGN(0);
    BOOST_CHECK(result.size() == 1);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result[0] == 0);

    result = SGN(1);
    BOOST_CHECK(result.size() == 1);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result[0] == 1);
}

//-----------------------------------------------------------------------------
// test export
//-----------------------------------------------------------------------------
#if HKU_SUPPORT_SERIALIZATION

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_SGN_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/SGN.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator x1 = SGN(CLOSE(kdata));
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(x1);
    }

    Indicator x2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(x2);
    }

    BOOST_CHECK(x1.size() == x2.size());
    BOOST_CHECK(x1.discard() == x2.discard());
    BOOST_CHECK(x1.getResultNumber() == x2.getResultNumber());
    for (size_t i = 0; i < x1.size(); ++i) {
        BOOST_CHECK_CLOSE(x1[i], x2[i], 0.00001);
    }
}
#endif /* #if HKU_SUPPORT_SERIALIZATION */

/** @} */


// libgp - Gaussian process library for Machine Learning
// Copyright (c) 2011, Manuel Blum <mblum@informatik.uni-freiburg.de>
// All rights reserved.
#include "cov_factory.h"
#include "cov.h"
#include "basis_functions/basisf_factory.h"
#include "basis_functions/IBasisFunction.h"

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

#if GTEST_HAS_PARAM_TEST

using ::testing::TestWithParam;
using ::testing::Values;

class BFGradientTest: public TestWithParam<std::string> {
protected:
	virtual void SetUp() {
		n = 3;
		M = 13;
		e = 1e-8;
		wrappedCovarianceFunction = covFactory.create(n,
				"CovSum ( CovSEard, CovNoise)");
		covf = factory.createBasisFunction(GetParam(), M,
				wrappedCovarianceFunction);
		param_dim = covf->get_param_dim();
		params = Eigen::VectorXd::Random(param_dim);
		x1 = Eigen::VectorXd::Random(n);
		x2 = Eigen::VectorXd::Random(n);
		covf->set_loghyper(params);
	}
	virtual void TearDown() {
		delete covf;
		delete wrappedCovarianceFunction;
	}
	int n, param_dim;
	size_t M;
	libgp::CovFactory covFactory;
	libgp::BasisFFactory factory;
	libgp::CovarianceFunction * wrappedCovarianceFunction;
	libgp::IBasisFunction * covf;
	double e;
	Eigen::VectorXd params;
	Eigen::VectorXd x1;
	Eigen::VectorXd x2;

	Eigen::VectorXd gradient() {
		Eigen::VectorXd grad(param_dim);
		covf->grad(x1, x2, grad);
		return grad;
	}

	Eigen::VectorXd gradient_diag() {
		Eigen::VectorXd grad(param_dim);
		covf->grad(x1, x1, grad);
		return grad;
	}

	double numerical_gradient(int i) {
		double theta = params(i);
		params(i) = theta - e;
		covf->set_loghyper(params);
		double j1 = covf->getWrappedKernelValue(x1, x2);
		params(i) = theta + e;
		covf->set_loghyper(params);
		double j2 = covf->getWrappedKernelValue(x1, x2);
		params(i) = theta;
		return (j2 - j1) / (2 * e);
	}

	double numerical_gradient_diag(int i) {
		double theta = params(i);
		params(i) = theta - e;
		covf->set_loghyper(params);
		double j1 = covf->getWrappedKernelValue(x1, x1);
		params(i) = theta + e;
		covf->set_loghyper(params);
		double j2 = covf->getWrappedKernelValue(x1, x1);
		params(i) = theta;
		return (j2 - j1) / (2 * e);
	}

    Eigen::MatrixXd numerical_gradient_of_isigma(size_t i){
        double theta = params(i);
        params(i) = theta - e;
        covf->set_loghyper(params);
        Eigen::MatrixXd j1 = covf->getInverseOfSigma();
        params(i) = theta + e;
        covf->set_loghyper(params);
        Eigen::MatrixXd j2 = covf->getInverseOfSigma();
        params(i) = theta;
        return ((j2.array()-j1.array())/(2*e)).matrix();
    }

	Eigen::VectorXd numerical_basis_function_gradient(size_t i) {
		double theta = params(i);
		params(i) = theta - e;
		covf->set_loghyper(params);
		Eigen::VectorXd j1 = covf->computeBasisFunctionVector(x1);
		params(i) = theta + e;
		covf->set_loghyper(params);
		Eigen::VectorXd j2 = covf->computeBasisFunctionVector(x1);
//		std::cout << "j1 - j2:" << ((j1-j2)/2/e).transpose().array() << std::endl;
		params(i) = theta;
		return (j2 - j1)/2/e;
	}
};

TEST_P(BFGradientTest, EqualToNumerical) {
	Eigen::VectorXd grad = gradient_diag();
	for (int i = 0; i < param_dim; ++i) {
		double num_grad = numerical_gradient(i);
		if (grad(i) == 0.0) {
			ASSERT_NEAR(num_grad, 0.0, 1e-2)<< "Parameter number: " << i
			<< std::endl << "numerical gradient: " << num_grad;
		}
		else {
			ASSERT_NEAR((num_grad-grad(i))/grad(i), 0.0, 1e-2) << "Parameter number: " << i
			<< std::endl << "numerical gradient: " << num_grad;
		}
	}
}

//TODO: refactor. copy&paste code!
TEST_P(BFGradientTest, DiagEqualToNumerical) {
	Eigen::VectorXd grad = gradient();
	for (int i = 0; i < param_dim; ++i) {
		double num_grad = numerical_gradient_diag(i);
		if (grad(i) == 0.0) {
			ASSERT_NEAR(num_grad, 0.0, 1e-2)<< "Parameter number: " << i
			<< std::endl << "numerical gradient: " << num_grad;
		}
		else {
			ASSERT_NEAR((num_grad-grad(i))/grad(i), 0.0, 1e-2) << "Parameter number: " << i
			<< std::endl << "numerical gradient: " << num_grad;
		}

	}
}

TEST_P(BFGradientTest, GradientOfiSigmaEqualToNumerical) {
	size_t M = covf->getNumberOfBasisFunctions();
  Eigen::MatrixXd grad(M, M);
  grad.setZero();
  for (int i=0; i<param_dim; ++i) {
	covf->gradiSigma(i, grad);
	Eigen::MatrixXd numeric_gradient = numerical_gradient_of_isigma(i);
	for(size_t j=0; j < M; j++){
		for(size_t k = 0; k < M; k++){
			if (grad(j, k) == 0.0) ASSERT_NEAR(numeric_gradient(j, k), 0.0, 1e-2);
			else ASSERT_NEAR((numeric_gradient(j, k)-grad(j, k))/grad(j, k), 0.0, 1e-2)
					<< "Parameter number: " << i
					<< std::endl << "numerical gradient: " << numeric_gradient(j, k)
					<< std::endl << "computed gradient: " << grad(j, k)
					<< std::endl << "index: " << j << "," << k;
		}
	}
  }
}

TEST_P(BFGradientTest, BasisFunctionEqualToNumerical) {
	size_t M = covf->getNumberOfBasisFunctions();
	Eigen::VectorXd phi = covf->computeBasisFunctionVector(x1);
	Eigen::VectorXd grad(M);
	grad.setZero();
	for (int i = 0; i < param_dim; i++) {
		covf->gradBasisFunction(x1, phi, i, grad);
		Eigen::VectorXd numeric_gradient = numerical_basis_function_gradient(i);
		for (size_t j = 0; j < M; j++) {
			if (grad(j) == 0.0) {
				ASSERT_NEAR(numeric_gradient(j), 0.0, 1e-2)<< "parameter: " << i<< std::endl
						<< "numerical gradient: " << numeric_gradient(j) << std::endl << "m: " << j;
			}
			else {
				ASSERT_NEAR((numeric_gradient(j)-grad(j))/grad(j), 0.0, 1e-2)<< "parameter: "
						<< i<< std::endl << "numerical gradient: " << numeric_gradient(j) << std::endl << "m: " << j;
			}
		}
	}
}

TEST_P(BFGradientTest, LogDeterminantCorrect) {
	//det = 0.5*log|Sigma|
	double det = covf->getLogDeterminantOfSigma();
	double det2 = -log(covf->getCholeskyOfInvertedSigma().diagonal().prod());

	if(covf->sigmaIsDiagonal()){
		double det_true = log(covf->getSigma().diagonal().prod())/2;
		//make sure the test values agree
		ASSERT_NEAR(det_true, det2, 1e-5);

		ASSERT_NEAR(det, det_true, 1e-5);
	}

	ASSERT_NEAR(det, det2, 1e-5);
}

TEST_P(BFGradientTest, CholeskyCorrect) {
	Eigen::MatrixXd iSigma = covf->getInverseOfSigma();
	Eigen::MatrixXd L = covf->getCholeskyOfInvertedSigma();
	//TODO: is this problematic?
	iSigma.array() = (iSigma - L*L.transpose()).array().abs();
	ASSERT_NEAR(iSigma.maxCoeff(), 0, 1e-15)
		<< "diff: " << std::endl << iSigma << std::endl
		<< "L: " << std::endl << L << std::endl;
}

TEST_P(BFGradientTest, InverseCorrect) {
	Eigen::MatrixXd iSigma = covf->getInverseOfSigma();
	Eigen::MatrixXd Sigma = covf->getSigma();
	Sigma = iSigma * Sigma;
	ASSERT_TRUE(Sigma.isDiagonal(1e-10));
}

#ifdef BUILD_FAST_FOOD
INSTANTIATE_TEST_CASE_P(BasisFunction, BFGradientTest,
		Values("SparseMultiScaleGP", "Solin", "FastFood"));
#else
INSTANTIATE_TEST_CASE_P(BasisFunction, BFGradientTest,
		Values("SparseMultiScaleGP", "Solin"));
#endif

#else

// Google Test may not support value-parameterized tests with some
// compilers. If we use conditional compilation to compile out all
// code referring to the gtest_main library, MSVC linker will not link
// that library at all and consequently complain about missing entry
// point defined in that library (fatal error LNK1561: entry point
// must be defined). This dummy test keeps gtest_main linked in.
TEST(DummyTest, ValueParameterizedTestsAreNotSupportedOnThisPlatform) {}

#endif  // GTEST_HAS_PARAM_TEST


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2008 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// This example constructs a mini-library for linear algebra, using
// expression templates to eliminate the need for temporaries when
// adding arrays of numbers. It duplicates the vector example from
// PETE (http://www.codesourcery.com/pooma/download.html)

#include "vector_kernal.hpp"
#include "tuple_kernal.hpp"

#include <geometry\vector.hpp>
#include <geometry\point.hpp>
#include <geometry\affine_space.hpp>
#include <geometry\indexed_access_traits.hpp>
#include <geometry\indexed_sequence_expression.hpp>

#include <vector>
#include <iostream>
#include <stdexcept>

#include <boost\test\unit_test.hpp>
#include <boost\test\floating_point_comparison.hpp>

// Here is indexed_sequence_expression, which extends a proto expr type by
// giving it an operator [] which uses the indexed_sequence_subscript_context
// to evaluate an expression with a given index.
template <typename T>
double access( T t, std::size_t i )
{
    using namespace generative::geometry;
    switch( i )
    {
        case 0:
        return indexed_access_traits< T >::get<0>( t );
        case 1:
        return indexed_access_traits< T >::get<1>( t );
        case 2:
        return indexed_access_traits< T >::get<2>( t );
    default:
        throw std::out_of_range( "" );
    }
}

template <typename Vector>
void TestVectorOperations()
{
    using namespace generative::geometry;
    using namespace generative::geometry::indexed_sequence_operators;

    BOOST_CHECK( (numeric_width_compare< int, double >::type::value) );
    BOOST_CHECK( (numeric_width_compare< float, double >::type::value) );

    BOOST_CHECK( !(numeric_width_compare< double, int >::type::value) );
    BOOST_CHECK( !(numeric_width_compare< double, float >::type::value) );

    vector_vector_3 a,b,c,d;
    Vector e = construct<Vector>(1,1,1);
    Vector f = construct<Vector>(2,2,2);

    for( int i = 0; i < 3; ++i)
    {
        a[i] = i;
        b[i] = (2*i);
        c[i] = (3*i);
        d[i] = (i);
    }

    int five = 5 * 1;

    BOOST_CHECK( is_indexed_sequence< Vector >::value );
    e << e + f;
    BOOST_CHECK_CLOSE( get<0>( e ), 3., 1e-10 );
    BOOST_CHECK_CLOSE( get<1>( e ), 3., 1e-10 );
    BOOST_CHECK_CLOSE( get<2>( e ), 3., 1e-10 );

    e << e * 4;
    BOOST_CHECK_CLOSE( get<0>( e ), 12., 1e-10 );
    BOOST_CHECK_CLOSE( get<1>( e ), 12., 1e-10 );
    BOOST_CHECK_CLOSE( get<2>( e ), 12., 1e-10 );

    typedef BOOST_TYPEOF( e - 4 / (c + 1) ) SpecificExpression;
    BOOST_AUTO( expr, e - 4 / (c + 1) );

    BOOST_CHECK( is_indexed_sequence< SpecificExpression >::value );

    double x0 = expr.get<0>();
    double x1 = x0 * 3;

    double x = get<0>( expr );
    BOOST_CHECK( x0 == x );
    std::cout << "x: " << x << std::endl;
    e << ( e - 4 / (c + 1) );

    Vector j = construct<Vector>(1,1,1);
    e << ( (j - 2) * f );
    e << ( e * 4.0 );
    for( int i = 0; i < 3; ++i)
    {
        std::cout
            << " a(" << i << ") = " << a[i]
            << " b(" << i << ") = " << b[i]
            << " c(" << i << ") = " << c[i]
            << " d(" << i << ") = " << d[i]
            << " e(" << i << ") = " << access( e, i )
            << std::endl;
    }

    //! Test doesn't interfere with regular shift.
    int t = 1;
    t = t << 1;
    BOOST_CHECK( t == 2 );
}

BOOST_AUTO_TEST_CASE( TestProtoExpressions )
{
    TestVectorOperations<vector_vector_3>();
    TestVectorOperations<vector_tuple_3>();
}


#define CATCH_CONFIG_MAIN
#include <Eigen/Dense>
#include <catch.hpp>
#include <random>

#include "yavque/Operators/SumPauliString.hpp"
#include "yavque/utils.hpp"

#include "common.hpp"

TEST_CASE("test commuting operators", "[test-commuting]")
{
	using namespace yavque;
	{
		std::map<uint32_t, Pauli> pstr1;
		std::map<uint32_t, Pauli> pstr2;

		pstr1[0] = Pauli('X');
		pstr1[1] = Pauli('X');

		pstr2[0] = Pauli('X');
		pstr2[1] = Pauli('X');

		REQUIRE(detail::commute(pstr1, pstr2));
	}
	{
		std::map<uint32_t, Pauli> pstr1;
		std::map<uint32_t, Pauli> pstr2;

		pstr1[0] = Pauli('X');
		pstr1[1] = Pauli('X');

		pstr2[0] = Pauli('Z');
		pstr2[1] = Pauli('Z');

		REQUIRE(detail::commute(pstr1, pstr2));
	}
	{
		std::map<uint32_t, Pauli> pstr1;
		std::map<uint32_t, Pauli> pstr2;

		pstr1[0] = Pauli('Z');
		pstr1[1] = Pauli('X');
		pstr1[2] = Pauli('Z');

		pstr2[1] = Pauli('Z');
		pstr2[2] = Pauli('X');
		pstr2[3] = Pauli('Z');

		REQUIRE(detail::commute(pstr1, pstr2));
	}
	{
		std::map<uint32_t, Pauli> pstr1;
		std::map<uint32_t, Pauli> pstr2;

		pstr1[0] = Pauli('Z');
		pstr1[1] = Pauli('X');
		pstr1[2] = Pauli('Z');

		pstr2[1] = Pauli('Z');
		pstr2[2] = Pauli('X');
		pstr2[3] = Pauli('Z');

		REQUIRE(detail::commute(pstr1, pstr2));
	}
}

TEST_CASE("test CompressedPauliString", "[pauli-string]")
{
	using namespace yavque;

	std::random_device rd;
	std::default_random_engine re{rd()};

	std::uniform_int_distribution<uint32_t> uid(0, 2);
	const uint32_t N = 12;

	std::vector<uint32_t> sites;
	sites.reserve(N);

	char pauli_names[3] = {'X', 'Y', 'Z'};

	for(uint32_t k = 0; k < N; ++k)
		sites.push_back(k);

	for(uint32_t str_len = 2; str_len < 5; ++str_len)
	{
		for(uint32_t k = 0; k < 100; ++k) // instance
		{
			std::random_shuffle(sites.begin(), sites.end());
			std::vector<uint32_t> indices(sites.begin(), sites.begin() + str_len);
			std::string pstr;
			for(uint32_t m = 0; m < str_len; ++m)
			{
				pstr.push_back(pauli_names[uid(re)]);
			}

			Eigen::VectorXcd input = Eigen::VectorXcd::Random(1u << N);
			input.normalize();

			Eigen::VectorXcd res = input;

			auto cps = detail::CompressedPauliString(pstr);

			Eigen::VectorXcd res1
				= cps.apply(indices, res); // apply pstr[0] to 0 and pstr[1] to 1
			for(uint32_t k = 0; k < str_len; ++k)
			{
				switch(pstr[k])
				{
				case 'X':
					res = apply_single_qubit(res, Eigen::MatrixXd(pauli_x()), indices[k]);
					break;
				case 'Y':
					res = apply_single_qubit(res, Eigen::MatrixXcd(pauli_y()),
					                         indices[k]);
					break;
				case 'Z':
					res = apply_single_qubit(res, Eigen::MatrixXd(pauli_z()), indices[k]);
					break;
				}
			}

			if((res - res1).norm() > 1e-6)
			{
				std::cout << input.transpose() << std::endl;
				std::cout << res.transpose() << std::endl;
				std::cout << res1.transpose() << std::endl;
				std::cout << pstr << std::endl;
				for(auto idx : indices)
					std::cout << idx << ", ";
				std::cout << std::endl;
			}
			REQUIRE((res - res1).norm() < 1e-6);
		}
	}
}
TEST_CASE("test SumPauliString", "[sum-pauli-string]")
{
	using namespace yavque;

	std::random_device rd;
	std::default_random_engine re{rd()};

	const uint32_t N = 10;
	const uint32_t n_terms = 5;

	std::uniform_int_distribution<uint32_t> uid(0, 2);
	std::uniform_int_distribution<uint32_t> string_len_dist(2, N - 1);

	std::vector<uint32_t> sites;
	sites.reserve(N);

	auto gen = [&uid, &re]()
	{
		return Pauli('X' + uid(re));
	};

	for(uint32_t k = 0; k < N; ++k)
		sites.push_back(k);

	for(uint32_t instance = 0; instance < 100; ++instance)
	{
		SumPauliString sps(N);

		std::vector<std::vector<uint32_t>> indices_v;
		std::vector<std::vector<Pauli>> pstring_v;

		for(uint32_t k = 0; k < n_terms; ++k)
		{
			uint32_t string_len = string_len_dist(re);
			std::shuffle(begin(sites), end(sites), re);
			std::vector<uint32_t> indices(sites.begin(), sites.begin() + string_len);

			std::vector<Pauli> pstring(string_len);
			std::generate(begin(pstring), end(pstring), gen);

			indices_v.push_back(indices);
			pstring_v.push_back(pstring);

			std::map<uint32_t, Pauli> pstr;
			std::transform(indices.begin(), indices.end(), pstring.begin(),
			               std::inserter(pstr, pstr.end()),
			               [](const auto& aa, const auto& bb)
			               { return std::make_pair(aa, bb); });

			sps += pstr;
		}

		Eigen::VectorXcd input = Eigen::VectorXcd::Random(1u << N);
		input.normalize();

		Eigen::VectorXcd res1 = sps.apply_right(input);
		Eigen::VectorXcd res2 = Eigen::VectorXcd::Zero(1u << N);

		for(uint32_t k = 0; k < n_terms; ++k)
		{
			auto cps = detail::CompressedPauliString(pstring_v[k]);
			res2 += cps.apply(indices_v[k], input);
		}

		REQUIRE((res1 - res2).norm() < 1e-6);
	}
}


#include <big_random.h>
#include <big_return_codes.h>
#include <big_types.h>
#include <min_map/big_compute_jacobian.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(big_compute_full_jacobian);

BOOST_AUTO_TEST_CASE(test_case1)
{
  typedef ublas::vector<size_t>            idx_vector_type;
  typedef ublas::compressed_matrix<double> matrix_type;

  matrix_type A;
  A.resize(10,10,false);

  big::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(0) = big::IN_NON_ACTIVE;
  bitmask(1) = big::IN_NON_ACTIVE;
  bitmask(2) = big::IN_ACTIVE;
  bitmask(3) = big::IN_NON_ACTIVE;
  bitmask(4) = big::IN_ACTIVE;
  bitmask(5) = big::IN_NON_ACTIVE;
  bitmask(6) = big::IN_ACTIVE;
  bitmask(7) = big::IN_NON_ACTIVE;
  bitmask(8) = big::IN_NON_ACTIVE;
  bitmask(9) = big::IN_ACTIVE;

  matrix_type J;
  big::compute_jacobian(
      A
    , bitmask
    , J
    );

  double tol = 0.01;

  // in-active
  BOOST_CHECK_CLOSE( double( J(0,0) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(1,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,1) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,9) ), double( 0.0 ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(2,0) ), double( A(2,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,1) ), double( A(2,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,2) ), double( A(2,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,3) ), double( A(2,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,4) ), double( A(2,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,5) ), double( A(2,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,6) ), double( A(2,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,7) ), double( A(2,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,8) ), double( A(2,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,9) ), double( A(2,9) ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(3,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,3) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,9) ), double( 0.0 ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(4,0) ), double( A(4,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,1) ), double( A(4,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,2) ), double( A(4,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,3) ), double( A(4,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,4) ), double( A(4,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,5) ), double( A(4,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,6) ), double( A(4,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,7) ), double( A(4,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,8) ), double( A(4,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,9) ), double( A(4,9) ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(5,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,5) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,9) ), double( 0.0 ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(6,0) ), double( A(6,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,1) ), double( A(6,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,2) ), double( A(6,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,3) ), double( A(6,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,4) ), double( A(6,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,5) ), double( A(6,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,6) ), double( A(6,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,7) ), double( A(6,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,8) ), double( A(6,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,9) ), double( A(6,9) ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(7,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,7) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(8,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,8) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,9) ), double( 0.0 ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(9,0) ), double( A(9,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,1) ), double( A(9,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,2) ), double( A(9,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,3) ), double( A(9,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,4) ), double( A(9,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,5) ), double( A(9,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,6) ), double( A(9,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,7) ), double( A(9,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,8) ), double( A(9,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,9) ), double( A(9,9) ), tol );

}

BOOST_AUTO_TEST_CASE(test_case2)
{
  typedef ublas::vector<size_t>            idx_vector_type;
  typedef ublas::compressed_matrix<double> matrix_type;

  matrix_type A;
  A.resize(10,10,false);

  big::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(0) = big::IN_NON_ACTIVE;
  bitmask(1) = big::IN_NON_ACTIVE;
  bitmask(2) = big::IN_NON_ACTIVE;
  bitmask(3) = big::IN_NON_ACTIVE;
  bitmask(4) = big::IN_NON_ACTIVE;
  bitmask(5) = big::IN_NON_ACTIVE;
  bitmask(6) = big::IN_NON_ACTIVE;
  bitmask(7) = big::IN_NON_ACTIVE;
  bitmask(8) = big::IN_NON_ACTIVE;
  bitmask(9) = big::IN_NON_ACTIVE;

  matrix_type J;
  big::compute_jacobian(
                        A
                        , bitmask
                        , J
                        );

  double tol = 0.01;

  // in-active
  BOOST_CHECK_CLOSE( double( J(0,0) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(1,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,1) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(2,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,2) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(2,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(3,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,3) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(4,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,4) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(4,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(5,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,5) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(6,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,6) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(6,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(7,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,7) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(8,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,8) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,9) ), double( 0.0 ), tol );

  // in-active
  BOOST_CHECK_CLOSE( double( J(9,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(9,9) ), double( 1.0 ), tol );

}

BOOST_AUTO_TEST_CASE(test_case3)
{
  typedef ublas::vector<size_t>            idx_vector_type;
  typedef ublas::compressed_matrix<double> matrix_type;

  matrix_type A;
  A.resize(10,10,false);

  big::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(0) = big::IN_ACTIVE;
  bitmask(1) = big::IN_ACTIVE;
  bitmask(2) = big::IN_ACTIVE;
  bitmask(3) = big::IN_ACTIVE;
  bitmask(4) = big::IN_ACTIVE;
  bitmask(5) = big::IN_ACTIVE;
  bitmask(6) = big::IN_ACTIVE;
  bitmask(7) = big::IN_ACTIVE;
  bitmask(8) = big::IN_ACTIVE;
  bitmask(9) = big::IN_ACTIVE;

  matrix_type J;
  big::compute_jacobian(
                        A
                        , bitmask
                        , J
                        );

  double tol = 0.01;

  // active
  BOOST_CHECK_CLOSE( double( J(0,0) ), double( A(0,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,1) ), double( A(0,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,2) ), double( A(0,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,3) ), double( A(0,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,4) ), double( A(0,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,5) ), double( A(0,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,6) ), double( A(0,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,7) ), double( A(0,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,8) ), double( A(0,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(0,9) ), double( A(0,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(1,0) ), double( A(1,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,1) ), double( A(1,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,2) ), double( A(1,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,3) ), double( A(1,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,4) ), double( A(1,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,5) ), double( A(1,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,6) ), double( A(1,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,7) ), double( A(1,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,8) ), double( A(1,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,9) ), double( A(1,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(2,0) ), double( A(2,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,1) ), double( A(2,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,2) ), double( A(2,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,3) ), double( A(2,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,4) ), double( A(2,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,5) ), double( A(2,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,6) ), double( A(2,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,7) ), double( A(2,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,8) ), double( A(2,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,9) ), double( A(2,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(3,0) ), double( A(3,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,1) ), double( A(3,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,2) ), double( A(3,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,3) ), double( A(3,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,4) ), double( A(3,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,5) ), double( A(3,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,6) ), double( A(3,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,7) ), double( A(3,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,8) ), double( A(3,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(3,9) ), double( A(3,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(4,0) ), double( A(4,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,1) ), double( A(4,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,2) ), double( A(4,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,3) ), double( A(4,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,4) ), double( A(4,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,5) ), double( A(4,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,6) ), double( A(4,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,7) ), double( A(4,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,8) ), double( A(4,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,9) ), double( A(4,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(5,0) ), double( A(5,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,1) ), double( A(5,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,2) ), double( A(5,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,3) ), double( A(5,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,4) ), double( A(5,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,5) ), double( A(5,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,6) ), double( A(5,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,7) ), double( A(5,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,8) ), double( A(5,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,9) ), double( A(5,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(6,0) ), double( A(6,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,1) ), double( A(6,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,2) ), double( A(6,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,3) ), double( A(6,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,4) ), double( A(6,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,5) ), double( A(6,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,6) ), double( A(6,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,7) ), double( A(6,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,8) ), double( A(6,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,9) ), double( A(6,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(7,0) ), double( A(7,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,1) ), double( A(7,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,2) ), double( A(7,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,3) ), double( A(7,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,4) ), double( A(7,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,5) ), double( A(7,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,6) ), double( A(7,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,7) ), double( A(7,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,8) ), double( A(7,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,9) ), double( A(7,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(8,0) ), double( A(8,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,1) ), double( A(8,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,2) ), double( A(8,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,3) ), double( A(8,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,4) ), double( A(8,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,5) ), double( A(8,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,6) ), double( A(8,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,7) ), double( A(8,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,8) ), double( A(8,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,9) ), double( A(8,9) ), tol );

  // active
  BOOST_CHECK_CLOSE( double( J(9,0) ), double( A(9,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,1) ), double( A(9,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,2) ), double( A(9,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,3) ), double( A(9,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,4) ), double( A(9,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,5) ), double( A(9,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,6) ), double( A(9,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,7) ), double( A(9,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,8) ), double( A(9,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,9) ), double( A(9,9) ), tol );
}

BOOST_AUTO_TEST_SUITE_END();


//igl
#include <igl/opengl/glfw/Viewer.h>
#include <igl/readMESH.h>
#include <igl/readOBJ.h>
#include <igl/writeOBJ.h>
#include <igl/per_vertex_normals.h>
#include <igl/per_corner_normals.h>
#include <igl/boundary_facets.h>
#include <igl/lbs_matrix.h>
#include <igl/deform_skeleton.h>
#include <igl/mat_max.h>
#include <igl/mat_min.h>
#include <igl/PI.h>
#include <igl/volume.h>
#include <igl/cat.h>
#include <igl/png/writePNG.h>

//internal
#include <json.hpp>
#include <read_data_from_json.h>
#include <lumped_mass_matrix.h>
#include <lbs_matrix.h>
#include <create_mask_matrix.h>
#include <line_search.h>
#include <util.h>

//Bartels
#include <linear_tetmesh_dphi_dX.h>
#include <linear_tetmesh_arap_dq.h>
#include <linear_tetmesh_arap_dq2.h>
#include <linear_tetmesh_arap_q.h>
#include <linear_tetmesh_neohookean_dq.h>
#include <linear_tetmesh_neohookean_dq2.h>
#include <linear_tetmesh_neohookean_q.h>
#include <linear_tetmesh_stvk_dq.h>
#include <linear_tetmesh_stvk_dq2.h>
#include <linear_tetmesh_stvk_q.h>
#include <linear_tetmesh_corotational_dq.h>
#include <linear_tetmesh_corotational_dq2.h>
#include <linear_tetmesh_corotational_q.h>
#include <simple_psd_fix.h>
#include <Eigen/Sparse>
#include <Eigen/Core>

#include <iostream>
#include <filesystem>

const Eigen::Vector3d red(255./255.,0./255.,0./255.);
int frame = 0;

void export_png_seq(igl::opengl::glfw::Viewer& viewer, const std::vector<Eigen::MatrixXd>& Vn_list,
                    const std::string& json_path, const std::string& physic_model)
{
  frame = 0;
  viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer &) -> bool
	{
		if(viewer.core().is_animating)
		{
      Eigen::MatrixXd Vn = Vn_list[frame];
      // viewer.data().set_mesh(Vn,F);
      viewer.data().set_vertices(Vn);
      viewer.data().compute_normals();

      const std::string dir = igl::dirname(json_path) + PATH_SEPARATOR;;
      size_t dot_found = json_path.find_last_of(".");
      std::string model = std::string(json_path.begin()+dir.size(),json_path.begin()+dot_found);

      std::string output_folder = "../showcases";
      std::string output_path = output_folder+"/"+model+"_"+physic_model;
      if(!std::filesystem::exists(output_path)){
        std::filesystem::create_directory(output_path);
      }

      std::string numstr = std::to_string(frame);
      int strlen = numstr.length();

      std::string seqstr;
      for(int k=0; k<(4-strlen); k++){
          seqstr.append("0");
      }
      seqstr.append(numstr);
      std::string save_path = output_path+"/"+model+"_"+physic_model+"_"+seqstr+".png";

      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> R(1280,800);
      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> G(1280,800);
      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> B(1280,800);
      Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> A(1280,800);

      // Draw the scene in the buffers
      viewer.core().draw_buffer(viewer.data(),true,R,G,B,A);
      igl::png::writePNG(R,G,B,A,save_path);

			frame++;
      if(frame == Vn_list.size()){
        viewer.core().is_animating = !viewer.core().is_animating;
        frame = 0;
      }
		}
		return false;
	};
}

int main(int argc, char *argv[])
{
  Eigen::MatrixXd V,C,W,VM; //V: vertices of tet-mesh, C: joint positions, W: skinning weight, VM: lbs matrix
  Eigen::MatrixXi T,F,BE; //T: tet indices  of tet-mesh, F: face indices of tet-mesh, BE: edge indices of bone handle
  Eigen::VectorXi PI; //PI: point indices of point handle

  std::vector<Eigen::MatrixXd> TF_list; //list of transformation
  double dt, YM, pr, scale; //dt: time step, YM: young's modulus, pr: poisson ratio, scale: custom scale of mesh

  std::string json_path = argc>1?std::string(argv[1]):"../examples/sphere/sphere.json";
  std::string physic_model;
  read_json_data(json_path,V,T,F,C,PI,BE,W,TF_list,dt,YM,pr,scale,physic_model);

  std::string str = argc>2?std::string(argv[2]):"";
  bool export_objs = false;
  if(str=="export")
    export_objs=true;

  double lambda, mu;
  emu_to_lame(YM,pr,lambda,mu);

  Eigen::MatrixXd params(T.rows(),2);
  params.col(0) = 0.5*lambda*Eigen::VectorXd::Ones(T.rows());
  params.col(1) = mu*Eigen::VectorXd::Ones(T.rows());

  igl::boundary_facets(T, F);
  F = F.rowwise().reverse().eval();

	igl::lbs_matrix(V,W,VM);

  Eigen::VectorXd vol;
  igl::volume(V,T,vol);

  Eigen::MatrixXd dX;
  sim::linear_tetmesh_dphi_dX(dX,V,T);

  Eigen::SparseMatrix<double> M;
  lumped_mass_matrix(V,T,M);
  M = M*1000;

  Eigen::SparseMatrix<double> A;
  lbs_matrix_column(V,W,A);

  Eigen::SparseMatrix<double> phi;
  create_poisson_mask_matrix(V,T,phi);

  Eigen::SparseMatrix<double> Aeq = A.transpose()*M*phi;
  Eigen::VectorXd Beq = Eigen::VectorXd::Zero(A.cols());

  Eigen::MatrixXd TF = TF_list[0];

  Eigen::MatrixXd Vr = VM*TF;
  Eigen::MatrixXd U = Vr-V;

  int nc = V.rows()*V.cols();

  Eigen::VectorXd UCol = vectorize(U);

  Eigen::VectorXd VCol = vectorize(V);

  Eigen::VectorXd UdCol = Eigen::VectorXd::Zero(nc);
  Eigen::VectorXd UcCol = Eigen::VectorXd::Zero(nc);

  std::vector<Eigen::MatrixXd> Vn_list(TF_list.size());
  int max_iter = 20;
  for(int ai=0; ai<TF_list.size(); ai++){
    std::cout<<"frame: "<<ai<<std::endl;

    Eigen::MatrixXd TF = TF_list[ai];

    Eigen::VectorXd UCol0 = UCol;
    Eigen::VectorXd UdCol0 = UdCol;
    Eigen::VectorXd UcCol0 = UcCol;

    Vr = VM*TF;
    Eigen::MatrixXd Ur = Vr-V;
    Eigen::VectorXd UrCol = vectorize(Ur);

    for(int i=0; i<max_iter; i++)
    {
      Eigen::VectorXd q = VCol+UrCol+UcCol;
      Eigen::VectorXd G;
      Eigen::SparseMatrix<double> K;

      if(physic_model=="arap"){
        sim::linear_tetmesh_arap_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_arap_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      else if(physic_model=="neohookean"){
        sim::linear_tetmesh_neohookean_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_neohookean_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      else if(physic_model=="stvk"){
        sim::linear_tetmesh_stvk_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_stvk_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      else if(physic_model=="corotational"){
        sim::linear_tetmesh_corotational_dq(G,V,T,q,dX,vol,params);
        sim::linear_tetmesh_corotational_dq2(K,V,T,q,dX,vol,params,[](auto &a) {sim::simple_psd_fix(a, 1e-3);});
      }
      Eigen::VectorXd tmp_g = M/(dt*dt) * (UrCol+UcCol) - M*(UCol0/(dt*dt)+UdCol0/dt) + G;
      Eigen::SparseMatrix<double> tmp_H = M/(dt*dt) + K;
      tmp_H = 0.5 * (tmp_H+Eigen::SparseMatrix<double>(tmp_H.transpose()));

      Eigen::SparseMatrix<double> AR1, AR2, AA;
      igl::cat(2,tmp_H,Eigen::SparseMatrix<double>(Aeq.transpose()),AR1);
      Eigen::SparseMatrix<double> SI(Aeq.rows(), Aeq.rows());
      SI.setZero();
      igl::cat(2,Aeq,SI,AR2);
      igl::cat(1,AR1,AR2,AA);

      Eigen::VectorXd b(tmp_g.size()+Beq.size());
      b.head(tmp_g.size())=-tmp_g;
      b.tail(Beq.size())=Beq;

      Eigen::SimplicialLDLT<Eigen::SparseMatrix<double > > ldlt(AA);
      Eigen::VectorXd dUc = ldlt.solve(b).head(tmp_g.size());

      if(tmp_g.transpose()*dUc > -1e-6)
        break;

      std::function<double(const Eigen::VectorXd &, const Eigen::VectorXd &)> f;
      f = [&](const Eigen::VectorXd &UrColi, const Eigen::VectorXd &UcColi){
        double e = 0.5*(UrColi+UcColi-UCol0-dt*UdCol0).transpose()*M/(dt*dt)*(UrColi+UcColi-UCol0-dt*UdCol0);
        if(physic_model=="arap"){
          return e + sim::linear_tetmesh_arap_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
        else if(physic_model=="neohookean"){
          return e + sim::linear_tetmesh_neohookean_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
        else if(physic_model=="stvk"){
          return e + sim::linear_tetmesh_stvk_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
        else if(physic_model=="corotational"){
          return e + sim::linear_tetmesh_corotational_q(V, T, VCol+UrColi+UcColi, dX, vol, params);
        }
      };
      double alpha = line_search(f,tmp_g,dUc,UrCol,UcCol);
      UcCol = UcCol + alpha * dUc;
    }
    UCol = UrCol + UcCol;
    UdCol = (UCol-UCol0)/dt;

    Eigen::MatrixXd Vn = V + matrixize(UCol);
    Vn_list[ai] = Vn;

  }
  if(export_objs)
  {
    const std::string dir = igl::dirname(json_path) + PATH_SEPARATOR;;
    size_t dot_found = json_path.find_last_of(".");
    std::string model = std::string(json_path.begin()+dir.size(),json_path.begin()+dot_found);

    std::string output_folder = "../output";
    if(!std::filesystem::exists(output_folder)){
      std::filesystem::create_directory(output_folder);
    }
    std::string output_path = output_folder+"/"+model+"_"+physic_model;
    if(!std::filesystem::exists(output_path)){
      std::filesystem::create_directory(output_path);
    }
    for(int ai=0; ai<Vn_list.size(); ai++)
    {
      Eigen::MatrixXd Vn = Vn_list[ai];
      std::string numstr = std::to_string(ai);
      int strlen = numstr.length();

      std::string seqstr;
      for(int k=0; k<(4-strlen); k++){
          seqstr.append("0");
      }
      seqstr.append(numstr);
      std::string save_path = output_path+"/"+model+"_"+physic_model+"_"+seqstr+".obj";
      igl::writeOBJ(save_path,Vn,F);
    }
  }

	igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.core().align_camera_center(V);
  viewer.core().is_animating = true;
  viewer.core().animation_max_fps = 24.;
  viewer.data().show_lines = true;
  viewer.data().show_overlay_depth = false;
  viewer.core().background_color=Eigen::Vector4f::Ones();
  viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer &) -> bool
	{
		if(viewer.core().is_animating)
		{
      Eigen::MatrixXd Vn = Vn_list[frame];
      viewer.data().set_vertices(Vn);
      viewer.data().compute_normals();
			frame++;
      if(frame == Vn_list.size()){
        //viewer.core().is_animating = !viewer.core().is_animating;
        frame = 0;
      }
		}
		return false;
	};

  viewer.callback_key_down = [&](igl::opengl::glfw::Viewer &, unsigned int key, int mod)
  {
    if(key==' '){
      viewer.core().is_animating = !viewer.core().is_animating;
    }
    if(key=='e' || key=='E'){
      std::cout<<"export png seq"<<std::endl;
      viewer.core().is_animating = true;
      export_png_seq(viewer,Vn_list,json_path,physic_model);
    }
    return false;
  };
  viewer.launch();
}


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
using namespace boost;
using namespace std;
typedef boost::adjacency_list<listS, vecS, bidirectionalS> mygraph;

int main()
{
	mygraph g;
	add_edge (0, 1, g);
	add_edge (0, 3, g);
	add_edge (1, 2, g);
	add_edge (2, 3, g);
	mygraph::vertex_iterator vertexIt, vertexEnd;
	mygraph::in_edge_iterator inedgeIt, inedgeEnd;
	mygraph::in_edge_iterator outedgeIt, outedgeEnd;
	tie(vertexIt, vertexEnd) = vertices(g);
	for (; vertexIt != vertexEnd; ++vertexIt)
	{
		cout << "incoming edges for " << *vertexIt << ": ";
		tie(inedgeIt, inedgeEnd) = in_edges(*vertexIt, g);
		for(; inedgeIt != inedgeEnd; ++inedgeIt)
		{
			cout << *inedgeIt << " ";
		}
		cout << "\n";
	}
	for (; vertexIt != vertexEnd; ++vertexIt)
	{
		cout << "out-edges for " << *vertexIt << ":" ;
		tie(outedgeIt, outedgeEnd) = out_edges(*vertexIt, g); // Similar to incoming edges
	}
}


// Copyright (c) 2022 hs293go
//
// This software is released under the MIT License.
// https://opensource.org/licenses/MIT

#include <pybind11/attr.h>
#include <pybind11/embed.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/pytypes.h>

#include <Eigen/Dense>
#include <cmath>

namespace py = pybind11;
int main() {
  // Prepare the data...
  const Eigen::VectorXd xv_data = Eigen::VectorXd::LinSpaced(10000, -4, 4);
  const Eigen::VectorXd yv_data =
      xv_data.unaryExpr([](double it) { return tgamma(it); });

  // Embed the interpreter in the current scope
  py::scoped_interpreter interp{};

  // Import the module, similar to `import matplotlib.pyplot as plt`
  auto plt = py::module::import("matplotlib.pyplot");

  // Pybind11 does namespace/member resolution by `().attr("name")`.
  // Bring in `plot` etc. like `from matplotlib.pyplot import plot`
  auto plot = plt.attr("plot");
  auto show = plt.attr("show");
  auto ylim = plt.attr("ylim");
  auto savefig = plt.attr("savefig");

  // Use the following py::array ctor to map an existing buffer
  // https://pybind11.readthedocs.io/en/stable/advanced/pycpp/numpy.html
  py::array xv(xv_data.size(), xv_data.data());
  py::array yv(yv_data.size(), yv_data.data());

  // plot and show!
  plot(xv, yv, "r--");
  ylim(-10, 10);
  savefig("res/readme.png");
  show();
}


// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.

// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived from
//    this software without specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "rbr_sdp_solver.h"

#include <ceres/rotation.h>

#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <limits>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

#include "bcm_sdp_solver.h"
#include "math/matrix_square_root.h"

namespace gopt {
namespace solver {

RBRSDPSolver::RBRSDPSolver(const size_t n, const size_t block_dim)
    : RBRSDPSolver(n, block_dim, solver::SDPSolverOptions()) {}

RBRSDPSolver::RBRSDPSolver(
    const size_t n, const size_t block_dim,
    const solver::SDPSolverOptions& options)
    : BCMSDPSolver(n, block_dim, options){
  X_ = Eigen::MatrixXd::Identity(dim_ * n, dim_ * n);
}

void RBRSDPSolver::Solve(solver::Summary& summary) {
  double prev_func_val = std::numeric_limits<double>::max();
  double cur_func_val = this->EvaluateFuncVal();
  double duration = 0.0;
  double error = 0.0;

  summary.begin_time = std::chrono::high_resolution_clock::now();
  while (summary.total_iterations_num < sdp_solver_options_.max_iterations) {
    if (sdp_solver_options_.verbose) {
      this->LogToStd(summary.total_iterations_num, prev_func_val, cur_func_val,
                     error, duration);
    }

    if (IsConverge(prev_func_val, cur_func_val, sdp_solver_options_.tolerance, &error)) {
      break;
    }

    // convergence rate? Take it for consideration.
    for (size_t k = 0; k < n_; k++) {
      // Eliminating the k-th row and column from Y to form Bk
      Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3 * (n_ - 1), 3 * (n_ - 1));
      this->ReformingB(k, B);

      // Eliminating the k-th column and all but the k-th row from R to form Wk
      Eigen::MatrixXd W = Eigen::MatrixXd::Zero(3 * (n_ - 1), 3);
      this->ReformingW(k, W);

      Eigen::MatrixXd B_multi_W = B * W;
      Eigen::MatrixXd WtBW = W.transpose() * B_multi_W;

      // FIXME: (chenyu) Solving matrix square root with
      // SVD and LDL^T would generate different result
      Eigen::MatrixXd WtBW_sqrt = MatrixSquareRoot(WtBW);
      // Eigen::MatrixXd WtBW_sqrt =
      // MatrixSquareRootForSemidefinitePositiveMat(WtBW);

      // FIXME: (chenyu) Eigen 3.3.0 is required for the use of
      // CompleteOrthogonalDecomposition<>
      // Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> cqr(WtBW_sqrt);
      // Eigen::Matrix3d moore_penrose_pseinv = cqr.pseudoInverse();
      Eigen::Matrix3d moore_penrose_pseinv = WtBW_sqrt.inverse();

      // compute S by fixing the error of Equ.(47) in Erikson's paper
      Eigen::MatrixXd S = -B_multi_W * moore_penrose_pseinv;

      // reordering X
      this->ReorderingUnknown(k, B, S);
    }

    summary.total_iterations_num++;
    duration = summary.Duration();

    // Update function value
    prev_func_val = cur_func_val;
    cur_func_val = this->EvaluateFuncVal();
  }

  summary.total_iterations_num++;
  if (sdp_solver_options_.verbose) {
    this->LogToStd(summary.total_iterations_num, prev_func_val, cur_func_val,
                   error, duration);
  }
}

double RBRSDPSolver::EvaluateFuncVal() const {
  return EvaluateFuncVal(X_);
}

double RBRSDPSolver::EvaluateFuncVal(const Eigen::MatrixXd& Y) const {
  return (Q_ * Y).trace();
}

void RBRSDPSolver::ReformingB(const size_t k, Eigen::MatrixXd& B) {
  size_t r = 0, c = 0;  // the row and column index of matrix B

  for (size_t i = 0; i < n_; i++) {
    if (i == k) continue;
    c = 0;
    for (size_t j = 0; j < n_; j++) {
      if (j == k) continue;

      B.block(3 * r, 3 * c, 3, 3) = X_.block(3 * i, 3 * j, 3, 3);
      c++;
    }
    r++;
  }
}

void RBRSDPSolver::ReformingW(const size_t k, Eigen::MatrixXd& W) {
  size_t r = 0;  // row index of matrix W

  for (size_t i = 0; i < n_; i++) {
    if (i == k) continue;

    W.block(3 * r, 0, 3, 3) = Q_.block(3 * i, 3 * k, 3, 3);
    r++;
  }
}

void RBRSDPSolver::ReorderingUnknown(const size_t k, const Eigen::MatrixXd& B,
                                     const Eigen::MatrixXd& S) {
  // Reordering X according to [Algorithm 1] in paper:
  // - Z. Wen, D. Goldfarb, S. Ma, and K. Scheinberg.
  //   Row by row methods for semidefinite programming.
  //   Technical report, Columbia University, 2009. 7

  // Update X(k, k)
  X_.block(3 * k, 3 * k, 3, 3) = Eigen::Matrix3d::Identity();

  // Update the k-th column of Y, except Y(k, k)
  size_t j = 0;  // the j-th block of S
  for (size_t i = 0; i < n_; i++) {
    if (i == k) continue;
    X_.block(3 * i, 3 * k, 3, 3) = S.block(3 * j, 0, 3, 3);
    j++;
  }

  // Update the k-th row of Y, except Y(k, k)
  size_t i = 0;  // the i-th block of S
  for (size_t j = 0; j < n_; j++) {
    if (j == k) continue;
    X_.block(3 * k, 3 * j, 3, 3) = S.block(3 * i, 0, 3, 3).transpose();
    i++;
  }
}

}  // namespace solver
}  // namespace gopt


/*
 * PoseOptimizationQP.cpp
 *
 *  Created on: Jun 10, 2015
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "free_gait_core/pose_optimization/PoseOptimizationQP.hpp"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <kindr/Core>
#include <numopt_quadprog/ActiveSetFunctionMinimizer.hpp>
#include <numopt_common/ParameterizationIdentity.hpp>

using namespace Eigen;

namespace free_gait {

PoseOptimizationQP::PoseOptimizationQP(const AdapterBase& adapter)
    : PoseOptimizationBase(adapter),
      nStates_(3),
      nDimensions_(3)
{
  solver_.reset(new numopt_quadprog::ActiveSetFunctionMinimizer());
}

PoseOptimizationQP::~PoseOptimizationQP()
{
}

PoseOptimizationQP::PoseOptimizationQP(const PoseOptimizationQP& other)
    : PoseOptimizationBase(other),
      nStates_(other.nStates_),
      nDimensions_(other.nDimensions_)
{
  solver_.reset(new numopt_quadprog::ActiveSetFunctionMinimizer());
}

bool PoseOptimizationQP::optimize(Pose& pose)
{
  checkSupportRegion();

  state_.setPoseBaseToWorld(pose);
  adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK.
  updateJointPositionsInState(state_);
  adapter_.setInternalDataFromState(state_, false, true, false, false);

  // Compute center of mass.
  const Position centerOfMassInBaseFrame(
      adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(),
                                 adapter_.getCenterOfMassInWorldFrame()));

  // Problem definition:
  // min Ax - b, Gx <= h
  unsigned int nFeet = stance_.size();
  MatrixXd A = MatrixXd::Zero(nDimensions_ * nFeet, nStates_);
  VectorXd b = VectorXd::Zero(nDimensions_ * nFeet);
  Matrix3d R = RotationMatrix(pose.getRotation()).matrix();

  unsigned int i = 0;
  for (const auto& footPosition : stance_) {
    A.block(nDimensions_ * i, 0, nStates_, A.cols()) << Matrix3d::Identity();
    b.segment(nDimensions_ * i, nDimensions_) << footPosition.second.vector() - R * nominalStanceInBaseFrame_[footPosition.first].vector();
    ++i;
  }

//  std::cout << "R: " << std::endl << R << std::endl;
//  std::cout << "A: " << std::endl << A << std::endl;
//  std::cout << "b: " << std::endl << b << std::endl;

  // Inequality constraints.
  Eigen::MatrixXd G;
  Eigen::VectorXd hp;
  supportRegion_.convertToInequalityConstraints(G, hp);
  Eigen::VectorXd h = hp - G * (R * centerOfMassInBaseFrame.vector()).head(2);
  G.conservativeResize(Eigen::NoChange,3); // Add column corresponding to Z position
  G.col(2).setZero(); // No constraints in Z position

//  std::cout << "G: " << std::endl << G << std::endl;
//  std::cout << "hp: " << std::endl << hp << std::endl;
//  std::cout << "h: " << std::endl << h << std::endl;

  // Formulation as QP:
  // min 1/2 x'Px + q'x + r
  MatrixXd P = 2 * A.transpose() * A;
  VectorXd q = -2 * A.transpose() * b;
//  MatrixXd r = b.transpose() * b; // Not used.

  // Cost function.
  auto costFunction = std::shared_ptr<numopt_common::QuadraticObjectiveFunction>(new numopt_common::QuadraticObjectiveFunction());
  numopt_common::SparseMatrix P_sparse = P.sparseView();
  costFunction->setGlobalHessian(P_sparse);
  costFunction->setLinearTerm(q);

  // Constraints.
  auto constraints = std::shared_ptr<numopt_common::LinearFunctionConstraints>(new numopt_common::LinearFunctionConstraints());
  numopt_common::SparseMatrix G_sparse = G.sparseView();
  constraints->setGlobalInequalityConstraintJacobian(G_sparse);
  constraints->setInequalityConstraintMinValues(std::numeric_limits<double>::lowest() * numopt_common::Vector::Ones(h.size()));
  constraints->setInequalityConstraintMaxValues(h);

  // Solve.
  numopt_common::QuadraticProblem problem(costFunction, constraints);

  Eigen::VectorXd x;
  numopt_common::ParameterizationIdentity params(x.size());
  params.getParams() = x;
  double cost = 0.0;
  if (!solver_->minimize(&problem, params, cost)) return false;
  x = params.getParams();
//  std::cout << "x: " << std::endl << x << std::endl;

  // Return optimized pose.
  pose.getPosition().vector() = x;
  return true;
}

} /* namespace */


#ifndef PYTHONIC_SCIPY_SPECIAL_SPHERICAL_YN_HPP
#define PYTHONIC_SCIPY_SPECIAL_SPHERICAL_YN_HPP

#include "pythonic/include/scipy/special/spherical_yn.hpp"

#include "pythonic/types/ndarray.hpp"
#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_traits.hpp"

#define BOOST_MATH_THREAD_LOCAL thread_local
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
    namespace details
    {
      template <class T0, class T1>
      double spherical_yn(T0 v, T1 x, bool derivative)
      {
        assert(v == (long)v &&
               "only supported for integral value as first arg");
        using namespace boost::math::policies;
        if (derivative) {
          return boost::math::sph_neumann_prime(
              v, x, make_policy(promote_double<true>()));
        } else {
          return boost::math::sph_neumann(v, x,
                                          make_policy(promote_double<true>()));
        }
      }
    }

#define NUMPY_NARY_FUNC_NAME spherical_yn
#define NUMPY_NARY_FUNC_SYM details::spherical_yn
#include "pythonic/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


//==================================================================================================
/*!
  @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_EXPONENTIAL_HPP_INCLUDED
#define BOOST_SIMD_EXPONENTIAL_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-exponential Exponential Functions

    These functions provide algorithms for computing exponentials and logarithms.
  **/

} }

#include <boost/simd/function/cbrt.hpp>
#include <boost/simd/function/exp10.hpp>
#include <boost/simd/function/exp2.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/expm1.hpp>
#include <boost/simd/function/exprecnegc.hpp>
#include <boost/simd/function/exprecneg.hpp>
#include <boost/simd/function/expx2.hpp>
#include <boost/simd/function/log10.hpp>
#include <boost/simd/function/log2.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/nthroot.hpp>
#include <boost/simd/function/pow2.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/function/pow.hpp>
#include <boost/simd/function/significants.hpp>

#endif


#include <Eigen>
#include <sandshark_common/main.h>
#include <sandshark_common/task_base.h>
#include <sandshark_common/proj_api.h>

#include <ros/ros.h>
#include <sandshark_msgs/OrientationQuaternion.h>
#include <sandshark_msgs/OrientationEuler.h>
#include <sandshark_msgs/MotionCorrected.h>
#include <sandshark_msgs/GPS.h>
#include <sandshark_msgs/Depth.h>
#include <sandshark_msgs/Altitude.h>
#include <sandshark_msgs/Speed.h>
#include <sandshark_msgs/Navigation.h>
#include <sandshark_msgs/Velocities.h>
#include <sys/socket.h>
#include <arpa/inet.h>

using namespace std;
using namespace Eigen;
namespace bluefin {
namespace sandshark {

class NavigationApp: public TaskBase {
  private:
    ros::Subscriber _orientEulerSub;
    ros::Subscriber _gpsSub;
    ros::Subscriber _depthSub;
    ros::Subscriber _altitudeSub;
    ros::Subscriber _dcSpeedSub;

    double _gpsNorthings, _gpsEastings, _gpsBearing, _gps_speed, _gpsEpoch;
    string _gpsGMTTimestamp;
    int _gpsUTMZone;
    bool _gpsUpdated;
    bool _gpsIsSouth;
    double _prevNorthings, _prevEastings;
    double _prevDepth, _prevAltitude;

    bool _prevValid;

    double _curYaw, _curRoll, _curPitch;
    double _prevYaw, _prevRoll, _prevPitch;
    bool _orientValid;

    double _tarePressure;
    bool _haveTare;
    double _depthPressure;
    ros::Time _lastDepthTime;

    double _altitude;
    ros::Time _lastAltTime;

    double _estimatedSpeed;
    ros::Time _lastSpeedTime;

    bool _dcSpeedValid;
    ros::Time _lastDeadReck;

    double _accelSpeed;
    bool _csValid;

    ros::Publisher _navPub;
    sandshark_msgs::Navigation _navMsg;

    ros::Publisher _velocitiesPub;
    sandshark_msgs::Velocities _velocitiesMsg;

    int _deadreckTest;
    double _ignoreGpsForXSeconds;
    double _prevDeadreckTestGPSEpoch;
    double _surfaceDepth;
    double _acceptableTranslationalWackness;
    double _acceptableRotationalWackness;
    double _maxAccelSpeed;

    bool _shouldCheckRots;

    bool _sendUDP;
    std::string _sendIPAddress;
    int _sendIPPort;
    int _sendSocket;
    struct sockaddr_in _sendAddrStruct;

    bool _initialized;
    int _utm_zone;
    projPJ _utmDefinition;
    projPJ _latlngDefinition;

    double calculateDepth(double latitude, double pressure);

    ros::Rate * _rate;

    void invalidateNavMsg(std::string s);

  protected:
    void startupInitCallback();
    bool doInitialize();
    bool doRun();
    void cleanup();
  public:
    NavigationApp() :
        TaskBase("Navigation", "navigation") {
    }

    void msgOrientEulerCallback(const sandshark_msgs::OrientationEuler::ConstPtr & msg);
    void msgGpsCallback(const sandshark_msgs::GPS::ConstPtr & msg);
    void msgDepthCallback(const sandshark_msgs::Depth::ConstPtr & msg);
    void msgAltitudeCallback(const sandshark_msgs::Altitude::ConstPtr & msg);
    void msgDCSpeedCallback(const sandshark_msgs::Speed::ConstPtr & msg);

    bool isWackness(std::string &wackness_reason);

    void handleSleep();
};

TaskBase *getTaskBase() {
  return new NavigationApp();
}

void NavigationApp::msgOrientEulerCallback(const sandshark_msgs::OrientationEuler::ConstPtr & msg) {
  _curYaw = msg->yaw;
  _curRoll = msg->roll;
  _curPitch = msg->pitch;
  _orientValid = true;
}

void NavigationApp::msgGpsCallback(const sandshark_msgs::GPS::ConstPtr & msg) {
  _gpsEastings = msg->eastings;
  _gpsNorthings = msg->northings;
  _gpsBearing = msg->bearing;
  _gps_speed = msg->speed;
  _gpsEpoch = msg->epoch;
  _gpsGMTTimestamp = msg->gmt_timestamp;
  _gpsUTMZone = msg->utm_zone;
  _gpsIsSouth = (msg->latitude < 0.0);
  _gpsUpdated = true;
}

void NavigationApp::msgDepthCallback(const sandshark_msgs::Depth::ConstPtr & msg) {
  _depthPressure = msg->pressure;
  _lastDepthTime = ros::Time::now();
}

void NavigationApp::msgAltitudeCallback(const sandshark_msgs::Altitude::ConstPtr & msg) {
  _altitude = msg->altitude;
  _lastAltTime = ros::Time::now();
}

void NavigationApp::msgDCSpeedCallback(const sandshark_msgs::Speed::ConstPtr & msg) {
  _estimatedSpeed = msg->speed_estimate;
  _lastSpeedTime = ros::Time::now();
  _dcSpeedValid = true;
}

void NavigationApp::startupInitCallback() {
  _orientEulerSub = _publicNode->subscribe("/motion/orientation_euler", 1, &NavigationApp::msgOrientEulerCallback,
      this);
  _gpsSub = _publicNode->subscribe("/gps/gps", 1, &NavigationApp::msgGpsCallback, this);
  _depthSub = _publicNode->subscribe("/depth/depth", 1, &NavigationApp::msgDepthCallback, this);
  _altitudeSub = _publicNode->subscribe("/altitude/altitude", 1, &NavigationApp::msgAltitudeCallback, this);
  _dcSpeedSub = _publicNode->subscribe("/dynamiccontrol/estimated_speed", 1, &NavigationApp::msgDCSpeedCallback, this);

  _navPub = _publicNode->advertise<sandshark_msgs::Navigation>("navState", 10);
  _velocitiesPub = _publicNode->advertise<sandshark_msgs::Velocities>("velocities", 10);

  _privateNode->param("deadrecktest", _deadreckTest, int(0));
  _privateNode->param("ignoreGpsForXSeconds", _ignoreGpsForXSeconds, double(120.0));
  _privateNode->param("surfaceDepth", _surfaceDepth, double(0.25));
  _privateNode->param("acceptableTranslationalWackness", _acceptableTranslationalWackness, double(1.0));
  _privateNode->param("acceptableRotationalWackness", _acceptableRotationalWackness, double(7.5));
  _prevDeadreckTestGPSEpoch = 0.0;

  _accelSpeed = 0.0;

  _shouldCheckRots = false;

  if (_logLocal && _logFile) {
    fprintf(_logFile,
        "ROSTime,\tGPSEpoch,\tNavEpoch,\tNavNorthings,\tNavEastings,\tGPSNorthings,\tGPSEastings,\tUTMZone,\tSpeed,\tBearing,\tPitch,\tRoll\n");
  }

  _privateNode->param("sendUDP", _sendUDP, false);
  _privateNode->param("sendIPAddress", _sendIPAddress, std::string("127.0.0.1"));
  _privateNode->param("sendIPPort", _sendIPPort, int(5556));
  ROS_INFO("SendIP %s : %d", _sendIPAddress.c_str(), _sendIPPort);

  _sendSocket = -1;
  if (_sendUDP) {
    _sendSocket = socket( AF_INET, SOCK_DGRAM, 0);
    if (_sendSocket < 0) {
      fprintf( stderr, "Socket is bad!\n");
      exit(-1);
    }

    struct sockaddr_in myaddr;
    memset((char *) &myaddr, 0, sizeof(myaddr));
    myaddr.sin_family = AF_INET;
    myaddr.sin_addr.s_addr = htonl( INADDR_ANY);
    myaddr.sin_port = htons(0);
    if (::bind(_sendSocket, (struct sockaddr *) &myaddr, sizeof(myaddr)) < 0) {
      fprintf( stderr, "bind failed! %d:%s\n", errno, strerror( errno));
      exit(-1);
    }

    memset(&_sendAddrStruct, 0, sizeof(_sendAddrStruct));
    _sendAddrStruct.sin_addr.s_addr = inet_addr(_sendIPAddress.c_str());
    _sendAddrStruct.sin_family = AF_INET;
    _sendAddrStruct.sin_port = htons(_sendIPPort);

  }
  _rate = new ros::Rate(20);
}

bool NavigationApp::doInitialize() {
  _gpsUpdated = false;
  _prevValid = false;
  _orientValid = false;
  _dcSpeedValid = false;

  _initialized = false;

  _prevAltitude = 0.0;
  _prevDepth = 0.0;
  return true;
}

void NavigationApp::cleanup() {
  //nothing to do...
}

bool NavigationApp::doRun() {
  static char sendBuf[1400];
  _navMsg.is_valid = true;
  _navMsg.invalid_reason = "";

  //ROS_INFO( "Nav::doRun Enter %f", ros::WallTime::now().toSec() );

  if (_deadreckTest > 0) {
    if (_gpsUpdated) {
      double elapsed = (_gpsEpoch - _prevDeadreckTestGPSEpoch) / 1000.0;
      if (elapsed >= _ignoreGpsForXSeconds) {
        _prevDeadreckTestGPSEpoch = _gpsEpoch;
      } else {
        _gpsUpdated = false;
      }
    }
  }

  if (_gpsUpdated) {
    _prevNorthings = _gpsNorthings;
    _prevEastings = _gpsEastings;
    _prevValid = true;
    _gpsUpdated = false;
    _navMsg.gmt_timestamp = _gpsGMTTimestamp;
    _navMsg.epoch = _gpsEpoch;
    _navMsg.quality = 1;
  } else {
    _navMsg.quality = 0;
  }

  if (_orientValid && _prevValid) {

    ros::Time now = ros::Time::now();
    ros::Duration deadReckDelta = now - _lastDeadReck;
    double deltaTime = deadReckDelta.toSec();

    _navMsg.computed_time = now;

    // Don't dead reckon if the delta is more than 0.4 seconds
    //TODO: may want to revisit how this is done
    if (deltaTime > 0.4) {
      deltaTime = 0.0;
    }

    //the speed estimate from dc already takes pitch into account
    double velX = 0.0;
    double velY = 0.0;

    if (_initialized && (now - _lastSpeedTime) < ros::Duration(5.0)) {
      velX = _estimatedSpeed * sin(((-1.0 * _curYaw) / 180.0) * M_PI);
      velY = _estimatedSpeed * cos(((-1.0 * _curYaw) / 180.0) * M_PI);

      _lastDeadReck = now;
    } else {
      //no more speed estimate, abort
      invalidateNavMsg("Nav stopped receiving speed estimate");
    }

    //only check for xy wackness if the vehicle is below surface depth, as it is perfectly resonable
    //to jump a large distance when receiving a first gps hit
    if (_navMsg.depth > _surfaceDepth) {
      double dx = velX * deltaTime;
      double dy = velY * deltaTime;
      if (dx > _acceptableTranslationalWackness || dx < -1 * _acceptableTranslationalWackness) {
        invalidateNavMsg("Preposterous change in eastings detected");
      }
      if (dy > _acceptableTranslationalWackness || dy < -1 * _acceptableTranslationalWackness) {
        invalidateNavMsg("Preposterous change in northings detected");
      }
    }

    _velocitiesMsg.east_velocity = velX;
    _velocitiesMsg.north_velocity = velY;

    _prevEastings += velX * deltaTime;
    _prevNorthings += velY * deltaTime;
    _navMsg.northings = _prevNorthings;
    _navMsg.eastings = _prevEastings;
    _navMsg.utm_zone = _gpsUTMZone;
    _navMsg.depth = 0.0;
    _navMsg.altitude = 0.0;
    _navMsg.gps_speed = _gps_speed;
    _navMsg.speed = _estimatedSpeed;

    //make sure no obscene changes in orientation occurred - no excuse for that, even on surface
    //only check after the first time
    if (_shouldCheckRots) {
      //yaw we need to account for wrapping at 360
      double wrappedYaw = 0.0;
      // if we have jumped too far, make nav invalid
      if (_curYaw > _prevYaw + _acceptableRotationalWackness
          || _curYaw < _prevYaw - _acceptableRotationalWackness) {
        //unless _currYaw and _prevYaw are close to 360 and/or 0
        if((0.0 + _acceptableRotationalWackness > _curYaw &&
            _curYaw > 0.0 - _acceptableRotationalWackness) &&
            (360.0 + _acceptableRotationalWackness > _prevYaw &&
             _prevYaw > 360.0 - _acceptableRotationalWackness)) {
        } else if((0.0 + _acceptableRotationalWackness > _prevYaw &&
            _prevYaw > 0.0 - _acceptableRotationalWackness) &&
            (360.0 + _acceptableRotationalWackness > _curYaw &&
             _curYaw > 360.0 - _acceptableRotationalWackness)) {
        } else {
          invalidateNavMsg("Preposterous change in Yaw detected");
        }
      }
      //no worries for pitch and roll wrapping.  If they wrap, there are
      //bigger issues to worry about
      if (_curPitch > _prevPitch + _acceptableRotationalWackness
          || _curPitch < _prevPitch - _acceptableRotationalWackness) {
        invalidateNavMsg("Preposterous change in Pitch detected");
      }
      if (_curRoll > _prevRoll + _acceptableRotationalWackness
          || _curRoll < _prevRoll - _acceptableRotationalWackness) {
        invalidateNavMsg("Preposterous change in Roll detected");
      }
    } else {
      _shouldCheckRots = true;
    }

    _prevYaw = _curYaw;
    _prevPitch = _curPitch;
    _prevRoll = _curRoll;

    _navMsg.bearing = -1.0 * _curYaw;
    if (_navMsg.bearing < 0.0) {
      _navMsg.bearing += 360.0;
    }
    if (_navMsg.bearing > 360.0) {
      _navMsg.bearing -= 360.0;
    }
    _navMsg.pitch = _curPitch;
    _navMsg.roll = _curRoll;

    if (!_initialized) {
      char utmInitString[256];
      snprintf(&utmInitString[0], sizeof(utmInitString), "+proj=utm +zone=%d +ellps=WGS84%s", _gpsUTMZone,
          (_gpsIsSouth ? " +south" : ""));

      _utmDefinition = pj_init_plus(utmInitString);
      if (!_utmDefinition) {
        ROS_ERROR("UTM Def failed!!\n");
        return false;
      }

      _latlngDefinition = pj_init_plus("+proj=latlong +ellps=WGS84");
      if (!_latlngDefinition) {
        ROS_ERROR("LatLng Def failed!!\n");
        return false;
      }

      _utm_zone = _gpsUTMZone;
      _initialized = true;
    }

    double lat = 0.0, lng = 0.0;
    bool latlng_valid = false;
    if (_initialized) {
      lat = _prevNorthings;
      lng = _prevEastings;
      int pret = pj_transform(_utmDefinition, _latlngDefinition, 1, 1, &lng, &lat, NULL);
      if (pret == 0) {
        latlng_valid = true;
      } else {
        latlng_valid = false;
        //ROS_ERROR("pj_transform returned invalid lat/lng - %d:%s", pret, pj_strerrno(pret));
      }
      lat *= RAD_TO_DEG;
      lng *= RAD_TO_DEG;
    }

    if (latlng_valid) {
      _navMsg.latitude = lat;
      _navMsg.longitude = lng;
    }

    now = ros::Time::now();

    if (_initialized && latlng_valid && (now - _lastDepthTime) < ros::Duration(0.5)) {
      _navMsg.depth = calculateDepth(lat, _depthPressure);
      //make sure we haven't had a ludicrous change in depth
      if (_navMsg.depth > _prevDepth + _acceptableTranslationalWackness
          || _navMsg.depth < _prevDepth - _acceptableTranslationalWackness) {
        invalidateNavMsg("Impossible change in depth detected");
      }
      _velocitiesMsg.down_velocity = (_navMsg.depth - _prevDepth) / deltaTime;
      _prevDepth = _navMsg.depth;
    } else if ((now - _lastDepthTime) >= ros::Duration(1.0)) {
      invalidateNavMsg("Nav stopped receiving depth data");
    }

    if (_logLocal && _logFile) {
      fprintf(_logFile, "%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%d,\t%f,\t%f,\t%f,\t%f,\t%f\n",
          ros::WallTime::now().toSec(), _gpsEpoch, _navMsg.epoch, _prevNorthings, _prevEastings, _gpsNorthings,
          _gpsEastings, _gpsBearing, _gpsUTMZone, _navMsg.speed, _curYaw, _navMsg.bearing, _navMsg.pitch, _navMsg.roll);
      fflush(_logFile);
    }

    if (_sendUDP) {
      snprintf(&sendBuf[0], sizeof(sendBuf), "%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%d,\t%f,\t%f,\t%f,\t%f,\t%f\n",
          ros::WallTime::now().toSec(), _gpsEpoch, _navMsg.epoch, _prevNorthings, _prevEastings, _gpsNorthings,
          _gpsEastings, _gpsBearing, _gpsUTMZone, _navMsg.speed, _curYaw, _navMsg.bearing, _navMsg.pitch, _navMsg.roll);

      sendBuf[sizeof(sendBuf) - 1] = '\0';
      int ret = sendto(_sendSocket, &sendBuf[0], strlen(sendBuf), 0, (struct sockaddr *) &_sendAddrStruct,
          sizeof(struct sockaddr));
      if (ret < 0) {
        fprintf( stderr, "SendTo failed ret %d - %d:%s\n", ret, errno, strerror( errno));
      }
    }

    if (_initialized && (now - _lastAltTime) < ros::Duration(10.0)) {
      _navMsg.altitude = _altitude;
      //altitude is somewhat filtered so if this happens its a huge red flag
      if (_altitude > _prevAltitude + _acceptableTranslationalWackness
          || _altitude < _prevAltitude - _acceptableTranslationalWackness) {
        invalidateNavMsg("Impossible change in altitude detected");
      }
      _prevAltitude = _altitude;
    } else {
      _navMsg.altitude = 0.0;
      _prevAltitude = 0.0;
      //if we haven't received altimiter data for 10 seconds, abort
      invalidateNavMsg("Nav stopped receiving altimiter data");
    }

    _navPub.publish(_navMsg);
    _orientValid = false;

    //TODO - need to do velocities if not dead reckoning
    _velocitiesPub.publish(_velocitiesMsg);
  }

  ROS_INFO( "Nav doRun exiting" );
  return true;
}

double NavigationApp::calculateDepth(double latitude, double pressure) {
  double depth_in_meters = 0.0;

  double lat_rad = latitude * DEG_TO_RAD;
  double pres_dbar = pressure; //Need conversion here!?

  if (!_haveTare) {
    _tarePressure = pres_dbar;
    _haveTare = true;
  }
  // Calculate depth in meters from latitude and pressure From
  // SBE Application Note 69
  double st2 = sin(lat_rad);
  st2 *= st2;
  double dbar = pres_dbar - _tarePressure;
  double C = (((-1.82e-15 * dbar + 2.279e-10) * dbar - 2.2512e-05) * dbar + 9.72659) * dbar;

  double gr = 9.780318 * ((2.3600e-05 * st2 + 5.2788e-3) * st2 + 1) + 1.092e-06 * dbar;
  depth_in_meters = C / gr; // meters
  //ROS_ERROR("PRESSURE IS %f DEPTH IS %f",pressure,depth_in_meters);

  return depth_in_meters;
}

void NavigationApp::handleSleep() {
  //ROS_INFO( "Nav::handleSleep enter" );
  _rate->sleep();
  //usleep(20000);
  //ROS_INFO( "Nav::handleSleep exiting" );
}

void NavigationApp::invalidateNavMsg(std::string s) {
  _navMsg.is_valid = false;
  _navMsg.invalid_reason = s;
  ROS_ERROR("Nav Validation False: %s\n", s.c_str());
}
}
}

int main(int argc, char *argv[]) {
  bluefin::sandshark::NavigationApp as;
  return bluefin::sandshark::app_main((bluefin::sandshark::TaskBase&) as, argc, argv);
}