#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Shape

// #include <iostream>

#include <random>
#include <boost/test/unit_test.hpp>
#include <geomc/function/Utils.h>
#include <geomc/shape/Oriented.h>
#include <geomc/shape/Cylinder.h>
#include <geomc/shape/Simplex.h>
#include <geomc/shape/Sphere.h>
#include <geomc/shape/Extrusion.h>
#include <geomc/shape/Frustum.h>

using namespace geom;
using namespace std;

typedef std::mt19937_64 rng_t;

// todo: tests:
//   - move a small distance away from a support point, both toward and away
//     from the object, and check for expected result of shape.contains()
//   - test that op(xf * shape, p) == op(shape, p / xf) for all {xf, p, shape, op}

// todo: test Frustum<Rect<T,1>>

rng_t rng(18374691138699945602ULL);

/****************************
 * random number generation *
 ****************************/

template <typename T, index_t N>
Vec<T,N> rnd(rng_t* rng) {
    std::normal_distribution<T> gauss(0, 1);
    Vec<T,N> v;
    for (index_t i = 0; i < N; ++i) {
        v[i] = gauss(*rng);
    }
    return v;
}

template <typename T>
inline T rnd(rng_t* rng) {
    std::normal_distribution<T> gauss(0, 1);
    return gauss(*rng);
}

/****************************
 * point sampling of shapes *
 ****************************/

template <typename Shape>
struct ShapeSampler {};

template <typename T, index_t N>
struct ShapeSampler<Simplex<T,N>> {
    Simplex<T,N> shape;

    ShapeSampler(const Simplex<T,N>& s):shape(s) {}

    Vec<T,N> operator()(rng_t* rng) {
        // algorithm from: https://projecteuclid.org/download/pdf_1/euclid.pjm/1102911301
        // we pick barycentric coordinates and then normalize so they sum to 1.
        std::exponential_distribution<T> e(1);
        T s[N + 1];
        T sum = 0;
        for (index_t i = 0; i < N + 1; ++i) {
            T xi = e(*rng);
            s[i] = xi;
            sum += xi;
        }
        Vec<T,N> p;
        for (index_t i = 0; i < N + 1; ++i) {
            T xi = s[i] / sum;
            p += xi * shape.pts[i];
        }
        return p;
    }
};

template <typename T, index_t N>
struct ShapeSampler<Rect<T,N>> {
    Rect<T,N> shape;
    ShapeSampler(const Rect<T,N>& s):shape(s) {}

    Vec<T,N> operator()(rng_t* rng) {
        std::uniform_real_distribution<T> u(0, 1);
        Vec<T,N> p;
        for (index_t i = 0; i < N; ++i) {
            p[i] = shape.lo[i] + u(*rng) * (shape.hi[i] - shape.lo[i]);
        }
        return p;
    }
};

template <typename T, index_t N>
struct ShapeSampler<Sphere<T,N>> {
    Sphere<T,N> shape;
    ShapeSampler(const Sphere<T,N>& s):shape(s) {}

    Vec<T,N> operator()(rng_t* rng) {
        std::uniform_real_distribution<T> u(0,1);
        Vec<T,N> p = rnd<T,N>(rng).unit();
        p = p * shape.r * std::pow(u(*rng), 1/(T)N) + shape.center;
        return p;
    }
};

template <typename T, index_t N>
struct ShapeSampler<Cylinder<T,N>> {
    Cylinder<T,N> shape;
    Vec<T,N> bases[N];

    ShapeSampler(const Cylinder<T,N>& s):shape(s) {
        bases[0] = s.p1 - s.p0;
        nullspace(bases, 1, bases + 1);
        orthonormalize(bases, N);
    }

    Vec<T,N> operator()(rng_t* rng) {
        std::uniform_real_distribution<T> u(0,1);
        Vec<T,N-1> px = ShapeSampler<Sphere<T,N-1>>(Sphere<T,N-1>(shape.radius))(rng);
        Vec<T,N> p = mix(u(*rng), shape.p0, shape.p1);
        for (index_t i = 0; i < N - 1; ++i) {
            p += bases[i + 1] * px[i];
        }
        return p;
    }
};

template <typename Shape>
struct ShapeSampler<Extrusion<Shape>> {
    typedef typename Shape::elem_t T;
    static constexpr size_t N = Shape::N + 1;

    Extrusion<Shape> shape;
    ShapeSampler(const Extrusion<Shape>& s):shape(s) {}

    Vec<T,N> operator()(rng_t* rng) {
        std::uniform_real_distribution<T> u(0,1);
        Vec<T,N-1> p = ShapeSampler<Shape>(shape.base)(rng);
        T h = (shape.height.hi - shape.height.lo) * u(*rng) + shape.height.lo;
        return Vec<T,N>(p, h);
    }
};

template <typename Shape>
struct ShapeSampler<Frustum<Shape>> {
    typedef typename Shape::elem_t T;
    static constexpr size_t N = Shape::N + 1;

    Frustum<Shape> shape;
    ShapeSampler(const Frustum<Shape>& s):shape(s) {}

    Vec<T,N> operator()(rng_t* rng) {
        // i am not positive this logic is right, but at worst
        // we just have a skewed sampling of our frustum
        std::uniform_real_distribution<T> u(0,1);
        Vec<T,N-1> p = ShapeSampler<Shape>(shape.base)(rng);
        auto c_h = shape.clipped_height();
        T v = std::sqrt(u(*rng));
          v = (c_h.lo < 0) ? (1 - v) : v;
        T h = (c_h.hi - c_h.lo) * v + c_h.lo;
        return Vec<T,N>(h * p, h);
    }
};

template <typename Shape>
struct ShapeSampler<Oriented<Shape>> {
    typedef typename Shape::elem_t T;
    static constexpr size_t N = Shape::N;

    Oriented<Shape> shape;
    ShapeSampler(const Oriented<Shape>& s):shape(s) {}

    Vec<T,N> operator()(rng_t* rng) {
        return shape.xf * ShapeSampler<Shape>(shape.shape)(rng);
    }
};

/****************************
 * random shape generation  *
 ****************************/

template <typename Shape>
struct RandomShape {};

template <typename T, index_t N>
struct RandomShape<Sphere<T,N>> {
    static Sphere<T,N> rnd_shape(rng_t* rng) {
        return Sphere<T,N>(10 * rnd<T,N>(rng), 5 * std::abs(rnd<T>(rng)));
    }
};

template <typename T, index_t N>
struct RandomShape<Cylinder<T,N>> {
    static Cylinder<T,N> rnd_shape(rng_t* rng) {
        // try not to be consistently centered on the origin:
        Vec<T,N> tx = 10 * rnd<T,N>(rng);
        return Cylinder<T,N>(
            5 * rnd<T,N>(rng) + tx,
            5 * rnd<T,N>(rng) + tx,
            2 * std::abs(rnd<T>(rng)));
    }
};

template <typename T, index_t N>
struct RandomShape<Rect<T,N>> {
    static Rect<T,N> rnd_shape(rng_t* rng) {
        Vec<T,N> tx = 10* rnd<T,N>(rng);
        return Rect<T,N>::spanning_corners(
            5 * rnd<T,N>(rng) + tx,
            5 * rnd<T,N>(rng) + tx
        );
    }
};

template <typename T, index_t N>
struct RandomShape<Simplex<T,N>> {
    static Simplex<T,N> rnd_shape(rng_t* rng) {
        Simplex<T,N> s;
        Vec<T,N> tx = 15 * rnd<T,N>(rng);
        for (index_t i = 0; i <= N; ++i) {
            s |= 5 * rnd<T,N>(rng) + tx;
        }
        return s;
    }
};

template <typename Shape>
struct RandomShape<Oriented<Shape>> {
    typedef typename Shape::elem_t T;
    static constexpr index_t N = Shape::N;

    static Oriented<Shape> rnd_shape(rng_t* rng) {
        SimpleMatrix<T,N,N> mx;
        for (index_t i = 0; i < N * N; ++i) {
            mx.begin()[i] = 3 * rnd<T>(rng);
        }
        AffineTransform<T,N> xf = translation(10 * rnd<T,N>(rng)) * transformation(mx);
        return Oriented<Shape>(RandomShape<Shape>::rnd_shape(rng), xf);
    }
};

template <typename Shape>
struct RandomShape<Frustum<Shape>> {
    typedef typename Shape::elem_t T;
    static constexpr index_t N = Shape::N;

    static Frustum<Shape> rnd_shape(rng_t* rng) {
        return Frustum<Shape>(
            RandomShape<Shape>::rnd_shape(rng),
            Rect<T,1>::spanning_corners(
                5 * rnd<T>(rng),
                5 * rnd<T>(rng)
            ));
    }
};

template <typename Shape>
struct RandomShape<Extrusion<Shape>> {
    typedef typename Shape::elem_t T;
    static constexpr index_t N = Shape::N;

    static Extrusion<Shape> rnd_shape(rng_t* rng) {
        return Extrusion<Shape>(
                RandomShape<Shape>::rnd_shape(rng),
                Rect<T,1>::spanning_corners(
                    5 * rnd<T>(rng),
                    5 * rnd<T>(rng)
                )
            );
    }
};

/****************************
 * shape calisthenics       *
 ****************************/

template <typename Shape, typename T, index_t N>
void validate_plane(const Shape& s, const Vec<T,N>& p, const Vec<T,N>& n) {
    Plane<T,N> pl = Plane<T,N>(n, s.convex_support(n));
    BOOST_CHECK(pl.contains(p));
}

template <typename Shape>
bool validate_point(
        rng_t* rng,
        const Shape& s,
        const Vec<typename Shape::elem_t,Shape::N>& p) {
    typedef typename Shape::elem_t T;
    constexpr index_t N = Shape::N;
    if (s.contains(p)) {
        // if the pt is in the shape, it should definitely be in the bbox.
        BOOST_CHECK(s.bounds().contains(p));
        // pick a random support direction. the point should be inside
        // the bounds of the resultant plane.
        validate_plane(s, p, rnd<T,N>(rng));

        // test all the cardinal axes as support directions. a valid point
        // should come back, and the plane through it should still contain p.
        // (sometimes cardinal axes can be perfectly aligned with faces
        // and this degeneracy might be handled poorly).
        Vec<T,N> a;
        for (index_t i = 0; i < N; ++i) {
            a[i] = 1;
            validate_plane(s, p, a);
            a[i] = -1;
            validate_plane(s, p, a);
            a[i] =  0;
        }
        return true;
    }
    return false;
}

template <typename Shape>
void exercise_shape(rng_t* rng, const Shape& s, index_t trials) {
    typedef typename Shape::elem_t T;
    constexpr index_t N = Shape::N;
    auto sampler   = ShapeSampler<Shape>(s);
    Rect<T,N> bbox = s.bounds();
    // shrink the blob so that more of it falls inside the bbox:
    Vec<T,N>  dims = bbox.dimensions() / 4;
    Vec<T,N>     c = bbox.center();

    for (index_t i = 0; i < trials; ++i) {
        // validate a point near the shape's bbox
        validate_point<Shape>(rng, s, rnd<T,N>(rng) * dims + c);
        // validate a point definitely in the shape
        BOOST_CHECK(validate_point<Shape>(rng, s, sampler(rng)));
        // validate a point near the origin
        validate_point<Shape>(rng, s, rnd<T,N>(rng));
    }
}

template <typename Shape>
void explore_shape(rng_t* rng, index_t shapes) {
    for (index_t i = 0; i < shapes; ++i) {
        Shape s = RandomShape<Shape>::rnd_shape(rng);
        exercise_shape<Shape>(rng, s, 50);
    }
}

template <typename T, index_t N>
void explore_simplex(rng_t* rng, index_t shapes) {
    for (index_t i = 0; i < shapes; ++i) {
        Simplex<T,N> splx = RandomShape<Simplex<T,N>>::rnd_shape(rng);
        ShapeSampler<Simplex<T,N>> smp(splx);
        for (index_t j = 0; j < 100; ++j) {
            Vec<T,N> p = smp(rng);
            BOOST_CHECK(splx.contains(p));
        }
    }
}

template <typename T, index_t N>
void test_simplex_projection(rng_t* rng, const Simplex<T,N>& s, index_t trials) {
    auto bb   = s.bounds();
    auto dims = bb.dimensions();
    auto ctr  = bb.center();
    // no degenerate boxes pls
    for (index_t i = 0; i < N; ++i) {
        if (dims[i] == 0) dims[i] = 1;
    }
    for (index_t i = 0; i < trials; ++i) {
        Simplex<T,N> ss;
        Vec<T,N> p  = rnd<T,N>(rng) * dims + ctr;
        Vec<T,N> pp = s.project(p, &ss);
        if (ss.n == N + 1) {
            // `p` is inside a full-volume simplex.
            // no points should have been excluded;
            // the "projected-to" simplex should be the same as the original
            BOOST_CHECK(ss == s);
            // if the point projected to the simplex volume, it should definitely
            // be contained by the simplex
            BOOST_CHECK(ss.contains(p));
            // the "projected" point should be precisely the original point
            BOOST_CHECK_EQUAL(p, pp);
        } else {
            // the direction from the surface pt to the original point
            // should be orthogonal to the simplex face
            Vec<T,N> v = p - pp;
            for (index_t j = 1; j < ss.n; ++j) {
                Vec<T,N> b = ss.pts[j] - ss.pts[0];
                BOOST_CHECK_SMALL(b.dot(v), 1e-5);
            }
            // the projected point should fall inside the face's bounds
            BOOST_CHECK(ss.projection_contains(p));
        }
    }
}

template <typename T, index_t N>
void exercise_simplex_projection(rng_t* rng, index_t trials) {
    for (index_t i = 0; i < trials; ++i) {
        Simplex<T,N> s = RandomShape<Simplex<T,N>>::rnd_shape(rng);
        std::uniform_int_distribution<> d(1,N+1);
        s.n = d(*rng); // make the simplex have a random number of verts
        test_simplex_projection(rng, s, 10);
    }
}

template <template <typename> class Outer, typename T>
void explore_compound_shape(rng_t* rng, index_t shapes) {
    explore_shape<Outer<Rect<T, 2>>>(rng, shapes);
    explore_shape<Outer<Rect<T, 3>>>(rng, shapes);
    explore_shape<Outer<Rect<T, 4>>>(rng, shapes);
    explore_shape<Outer<Rect<T, 5>>>(rng, shapes);

    // explore_shape<Outer<Cylinder<T, 2>>>(rng, shapes); // [1]
    explore_shape<Outer<Cylinder<T, 3>>>(rng, shapes);
    explore_shape<Outer<Cylinder<T, 4>>>(rng, shapes);
    explore_shape<Outer<Cylinder<T, 5>>>(rng, shapes);
    explore_shape<Outer<Cylinder<T, 7>>>(rng, shapes);

    explore_shape<Outer<Sphere<T, 2>>>(rng, shapes);
    explore_shape<Outer<Sphere<T, 3>>>(rng, shapes);
    explore_shape<Outer<Sphere<T, 4>>>(rng, shapes);
    explore_shape<Outer<Sphere<T, 5>>>(rng, shapes);
    explore_shape<Outer<Sphere<T, 7>>>(rng, shapes);

    explore_shape<Outer<Simplex<T, 2>>>(rng, shapes);
    explore_shape<Outer<Simplex<T, 3>>>(rng, shapes);
    explore_shape<Outer<Simplex<T, 4>>>(rng, shapes);
    explore_shape<Outer<Simplex<T, 5>>>(rng, shapes);
    explore_shape<Outer<Simplex<T, 7>>>(rng, shapes);

    // [1] A valid construction, but not currently tested
    // because the sampling code for Cylinder needs to
    // draw from an N-1 sphere. When N=2, this is a line,
    // which degenerates to a different point_t, and Sphere
    // is not set up to work with N=1.
}

/****************************
 * test cases               *
 ****************************/

BOOST_AUTO_TEST_SUITE(shape)

BOOST_AUTO_TEST_CASE(validate_rect) {
    explore_shape<Rect<double, 2>>(&rng, 1000);
    explore_shape<Rect<double, 3>>(&rng, 1000);
    explore_shape<Rect<double, 4>>(&rng, 1000);
    explore_shape<Rect<double, 5>>(&rng, 1000);
}

BOOST_AUTO_TEST_CASE(validate_cylinder) {
    explore_shape<Cylinder<double, 3>>(&rng, 1000);
    explore_shape<Cylinder<double, 4>>(&rng, 1000);
    explore_shape<Cylinder<double, 5>>(&rng, 1000);
    explore_shape<Cylinder<double, 7>>(&rng, 1000);
}

BOOST_AUTO_TEST_CASE(validate_simplex) {
    explore_shape<Simplex<double, 2>>(&rng, 1000);
    explore_shape<Simplex<double, 3>>(&rng, 1000);
    explore_shape<Simplex<double, 4>>(&rng, 1000);
    explore_shape<Simplex<double, 5>>(&rng, 1000);
    explore_shape<Simplex<double, 7>>(&rng, 1000);
    // todo: also check that contains() and projection_contains(),
    //       all agree about pt containment.
}

BOOST_AUTO_TEST_CASE(validate_sphere) {
    explore_shape<Sphere<double, 2>>(&rng, 1000);
    explore_shape<Sphere<double, 3>>(&rng, 1000);
    explore_shape<Sphere<double, 4>>(&rng, 1000);
}

BOOST_AUTO_TEST_CASE(validate_extrusion) {
    explore_compound_shape<Extrusion, double>(&rng, 250);
}

BOOST_AUTO_TEST_CASE(validate_oriented) {
    explore_compound_shape<Oriented, double>(&rng, 250);
}

BOOST_AUTO_TEST_CASE(validate_frustum) {
    explore_compound_shape<Frustum, double>(&rng, 250);
}

BOOST_AUTO_TEST_CASE(create_oriented_cylinder) {
    // make a null-transformed oriented cylinder.
    // (the cylinder defaults to unit radius and length along X)
    auto ocyl = Oriented<Cylinder<double,3>>(Cylinder<double,3>());
    // confirm that the Oriented delegates containment checking
    BOOST_CHECK(ocyl.contains(Vec3d(0.5, 0, 0)));
    // confirm the Oriented delegages convex_support
    BOOST_CHECK_EQUAL(ocyl.convex_support(Vec3d(0.1, 1, 0)), Vec3d(1, 1, 0));
    // rotate the thing 180 degrees
    ocyl *= rotation(Vec3d(0, 0, 1), M_PI);
    // confirm that wrapper applies the xf:
    BOOST_CHECK(ocyl.contains(Vec3d(-0.5, 0, 0)));
}

BOOST_AUTO_TEST_CASE(orient_simple_shape) {
    auto xf = translation(Vec3d(-5, 0, 0));
    // confirm the operator works and its return type is correct:
    Oriented<Cylinder<double,3>> ocyl = xf * Cylinder<double,3>();
    // confirm the created wrapper applies the xf:
    BOOST_CHECK(ocyl.contains(Vec3d(-4.5, 0, 0)));
    // verify inheritance
    Convex<double,3>* s = &ocyl;
    s->convex_support(Vec3d(0.2,0.4,0.1));
}

BOOST_AUTO_TEST_CASE(simplex_projection) {
    exercise_simplex_projection<double,2>(&rng, 1000);
    exercise_simplex_projection<double,3>(&rng, 1000);
    exercise_simplex_projection<double,4>(&rng, 1000);
    exercise_simplex_projection<double,5>(&rng, 1000);
    exercise_simplex_projection<double,7>(&rng, 1000);
}

BOOST_AUTO_TEST_SUITE_END()


/******************************************************************************

  Principal component analysis implementation using the covariance method.

  Copyright (c) 2012, 2013
  Alexander Rukletsov <rukletsov@gmail.com>
  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.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef PCA_HPP_C2EA373C_F360_43BA_BACB_4B26B78759BE_
#define PCA_HPP_C2EA373C_F360_43BA_BACB_4B26B78759BE_

#include <vector>
#include <algorithm>
#include <functional>
#include <stdexcept>
#include <boost/tuple/tuple.hpp>
#include <boost/array.hpp>
#include <boost/assert.hpp>

#include "bo/core/vector.hpp"
#include "bo/math/mean.hpp"
#include "bo/math/blas_extensions.hpp"
#include "bo/math/blas_conversions.hpp"

namespace bo {
namespace math {

using namespace boost::numeric::ublas;

// Performs PCA for the given data. Returns a tuple of eigenvalues and eigenvectors.
// Implemented as a functor in order to store induced typedefs inside.
template <typename RealType, std::size_t Dim>
struct PCA
{
    typedef Vector<RealType, Dim> Sample;
    typedef std::vector<Sample> Samples;

    typedef RealType EigenValue;
    typedef boost::array<EigenValue, Dim> EigenValues;
    typedef Vector<RealType, Dim> EigenVector;
    typedef boost::array<EigenVector, Dim> EigenVectors;
    typedef boost::tuples::tuple<EigenValues, EigenVectors> Result;

    Result operator() (Samples data)
    {
        typedef math::bounded_vector<RealType, Dim> BlasVector;
        typedef std::vector<RealType> StdVector;
        typedef math::bounded_matrix<RealType, Dim, Dim> Matrix;

        // If the data is empty, PCA is meaningless.
        if (data.size() == 0)
            throw std::logic_error("PCA for an empty set is meaningless.");

        // Find the mean among the neighbours.
        Sample mean_value = bo::math::mean(data);

        // Calculate the deviations from mean.
        std::transform(data.begin(), data.end(), data.begin(),
                       std::bind2nd(std::minus<Sample>(), mean_value));

        // Initialize covariance matrix.
        Matrix covar_matrix = math::zero_matrix<RealType>(Dim);

        // Iteratively compute the Dim x Dim covariance matrix (sample by sample).
        for (typename Samples::const_iterator pt = data.begin(); pt != data.end(); ++pt)
        {
            BlasVector vect = math::from_bo_vector(*pt);
            covar_matrix += math::outer_prod(vect, vect);
        }

        covar_matrix /= data.size();

        // Get the eigenvectors of the covariance matrix.
        StdVector covar_eigenvalues = math::eigen_symmetric(covar_matrix);
        BOOST_ASSERT((covar_eigenvalues.size() == Dim) &&
                     "Eigenvalues count differs from samples' dimensions.");

        // Convert eigenvectors to the output format.
        EigenValues eigenvalues;
        std::copy(covar_eigenvalues.begin(), covar_eigenvalues.end(), eigenvalues.begin());

        // Extract eignvectors from the columns of the updated covariance matrix and
        // convert them to the output format.
        EigenVectors eigenvectors;
        for (std::size_t col_idx = 0; col_idx < Dim; ++col_idx)
            eigenvectors[col_idx] = math::to_bo_vector(BlasVector(math::column(covar_matrix, col_idx)));

        // Put eigenvalues and eigenvectors into a tuple and return.
        return
            boost::tuples::make_tuple(eigenvalues, eigenvectors);
    }
};

} // namespace math
} // namespace bo

#endif // PCA_HPP_C2EA373C_F360_43BA_BACB_4B26B78759BE_


#include <gtest/gtest.h>

#include "mfem.hpp"
using namespace mfem;

#include <iostream>
#include <fstream>
#include <random>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "../include/core/config.hpp"
#include "../include/stokes/assembly.hpp"
#include "../include/incompNS/test_cases_factory.hpp"
#include "../include/incompNS/coefficients.hpp"
#include "../include/incompNS/observer.hpp"
#include "../include/incompNS/utilities.hpp"
#include "../include/uq/sampler/sampler.hpp"

TEST (IncompNSUtils, measureDivergenceQuadMesh)
{
    // config
    std::string filename
            = "../config_files/unit_tests/"
              "incompNS_svs.json";
    auto config = get_global_config(filename);

    // mesh file
    std::string base_mesh_dir("../meshes/");
    std::string sub_mesh_dir = config["mesh_dir"];
    const std::string mesh_dir = base_mesh_dir+sub_mesh_dir;
    const std::string mesh_file
            = mesh_dir+"quad_mesh_l0.mesh";
    const int lx = config["level_x"];

    // test case
    std::shared_ptr<IncompNSTestCases> testCase
        = make_incompNS_test_case(config);

    // mesh
    std::shared_ptr<Mesh> mesh
            = std::make_shared<Mesh>(mesh_file.c_str());
    for (int k=0; k<lx; k++) {
        mesh->UniformRefinement();
    }

    // FE spaces
    int deg = config["deg_x"];
    int ndim = mesh->Dimension();
    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(deg, ndim);
    FiniteElementSpace *R_space
            = new FiniteElementSpace(mesh.get(), hdiv_coll);

    FiniteElementCollection *l2_coll
            = new L2_FECollection(deg, ndim);
    FiniteElementSpace *W_space
            = new FiniteElementSpace(mesh.get(), l2_coll);

    // Divergence operator
    // \int_{\Omega} div(u_h) q_h d_{\Omega}
    MixedBilinearForm *div_form
            = new MixedBilinearForm(R_space,
                                    W_space);
    ConstantCoefficient one(-1.0);
    div_form->AddDomainIntegrator
            (new VectorFEDivergenceIntegrator(one));
    div_form->Assemble();
    div_form->Finalize();
    SparseMatrix *div = div_form->LoseMat();
    delete div_form;

    // velocity
    std::shared_ptr <GridFunction> v
            = std::make_shared<GridFunction>(R_space);
    Sampler<Uniform> unifSampler(10, 1);
    Vector omegas = unifSampler.generate_one_sample();
    IncompNSInitialVelocityCoeff v0_coeff(testCase);
    v0_coeff.SetTime(0);
    v->ProjectCoefficient(v0_coeff);

    double divVal = measure_divergence(div, v.get());

    double TOL=1E-8;
    ASSERT_LE(divVal, TOL);
}

TEST (IncompNSUtils, measureDivergenceTriMesh)
{
    // config
    std::string filename
            = "../config_files/unit_tests/"
              "incompNS_svs.json";
    auto config = get_global_config(filename);

    // mesh file
    std::string base_mesh_dir("../meshes/");
    std::string sub_mesh_dir = config["mesh_dir"];
    const std::string mesh_dir = base_mesh_dir+sub_mesh_dir;
    const std::string mesh_file
            = mesh_dir+"tri_mesh_l0.mesh";
    const int lx = config["level_x"];

    // test case
    std::shared_ptr<IncompNSTestCases> testCase
        = make_incompNS_test_case(config);

    // mesh
    std::shared_ptr<Mesh> mesh
            = std::make_shared<Mesh>(mesh_file.c_str());
    for (int k=0; k<lx; k++) {
        mesh->UniformRefinement();
    }

    // FE spaces
    int deg = config["deg_x"];
    int ndim = mesh->Dimension();
    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(deg, ndim);
    FiniteElementSpace *R_space
            = new FiniteElementSpace(mesh.get(), hdiv_coll);

    FiniteElementCollection *l2_coll
            = new L2_FECollection(deg, ndim);
    FiniteElementSpace *W_space
            = new FiniteElementSpace(mesh.get(), l2_coll);

    // Divergence operator
    // \int_{\Omega} div(u_h) q_h d_{\Omega}
    MixedBilinearForm *div_form
            = new MixedBilinearForm(R_space,
                                    W_space);
    ConstantCoefficient one(-1.0);
    div_form->AddDomainIntegrator
            (new VectorFEDivergenceIntegrator(one));
    div_form->Assemble();
    div_form->Finalize();
    SparseMatrix *div = div_form->LoseMat();
    delete div_form;

    // velocity
    std::shared_ptr <GridFunction> v
            = std::make_shared<GridFunction>(R_space);
    Sampler<Uniform> unifSampler(10, 1);
    Vector omegas = unifSampler.generate_one_sample();
    IncompNSInitialVelocityCoeff v0_coeff(testCase);
    v0_coeff.SetTime(0);
    v->ProjectCoefficient(v0_coeff);

    double divVal = measure_divergence(div, v.get());

    double TOL=2E-5;
    ASSERT_LE(divVal, TOL);
}

TEST (IncompNSUtils, divgFreeVelQuadMesh)
{
    // config
    std::string filename
            = "../config_files/unit_tests/"
              "incompNS_svs.json";
    auto config = get_global_config(filename);

    // mesh file
    std::string base_mesh_dir("../meshes/");
    std::string sub_mesh_dir = config["mesh_dir"];
    const std::string mesh_dir = base_mesh_dir+sub_mesh_dir;
    const std::string mesh_file
            = mesh_dir+"quad_mesh_l0.mesh";

    // test case
    std::shared_ptr<IncompNSTestCases> testCase
        = make_incompNS_test_case(config);

    // mesh
    const int lx = config["level_x"];
    std::shared_ptr<Mesh> mesh
            = std::make_shared<Mesh>(mesh_file.c_str());
    for (int k=0; k<lx; k++) {
        mesh->UniformRefinement();
    }

    // FE spaces
    int deg = config["deg_x"];
    int ndim = mesh->Dimension();
    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(deg, ndim);
    FiniteElementSpace *R_space
            = new FiniteElementSpace(mesh.get(), hdiv_coll);

    FiniteElementCollection *l2_coll
            = new L2_FECollection(deg, ndim);
    FiniteElementSpace *W_space
            = new FiniteElementSpace(mesh.get(), l2_coll);

    // Divergence operator
    // \int_{\Omega} div(u_h) q_h d_{\Omega}
    MixedBilinearForm *div_form
            = new MixedBilinearForm(R_space,
                                    W_space);
    ConstantCoefficient one(-1.0);
    div_form->AddDomainIntegrator
            (new VectorFEDivergenceIntegrator(one));
    div_form->Assemble();
    div_form->Finalize();
    SparseMatrix *div = div_form->LoseMat();
    delete div_form;

    // velocity
    std::shared_ptr <GridFunction> v
            = std::make_shared<GridFunction>(R_space);
    Sampler<Uniform> unifSampler(10, 1);
    Vector omegas = unifSampler.generate_one_sample();
    IncompNSInitialVelocityCoeff v0_coeff(testCase);
    testCase->set_perturbations(omegas);
    v0_coeff.SetTime(0);
    v->ProjectCoefficient(v0_coeff);

    // oberver
    std::shared_ptr<IncompNSObserver> observer
            = std::make_shared<IncompNSObserver>
            (config, lx);
    //(*observer)(v);

    double div_old = measure_divergence(div, v.get());
    std::cout << "Weak divergence before cleaning: "
              << div_old << std::endl;

    // make divergence free
    DivergenceFreeVelocity divFreeVel (config, mesh);
    divFreeVel (v.get());
    //(*observer)(v);

    double div_new = measure_divergence(div, v.get());
    std::cout << "Weak divergence after cleaning: "
              << div_new << std::endl;

    double TOL=1E-5;
    ASSERT_LE(div_new, TOL);
}

TEST (IncompNSUtils, divgFreeVelTriMesh)
{
    // config
    std::string filename
            = "../config_files/unit_tests/"
              "incompNS_svs.json";
    auto config = get_global_config(filename);

    // mesh file
    std::string base_mesh_dir("../meshes/");
    std::string sub_mesh_dir = config["mesh_dir"];
    const std::string mesh_dir = base_mesh_dir+sub_mesh_dir;
    const std::string mesh_file
            = mesh_dir+"tri_mesh_l0.mesh";

    // test case
    std::shared_ptr<IncompNSTestCases> testCase
        = make_incompNS_test_case(config);

    // mesh
    const int lx = config["level_x"];
    std::shared_ptr<Mesh> mesh
            = std::make_shared<Mesh>(mesh_file.c_str());
    for (int k=0; k<lx; k++) {
        mesh->UniformRefinement();
    }

    // FE spaces
    int deg = config["deg_x"];
    int ndim = mesh->Dimension();
    FiniteElementCollection *hdiv_coll
            = new RT_FECollection(deg, ndim);
    FiniteElementSpace *R_space
            = new FiniteElementSpace(mesh.get(), hdiv_coll);

    FiniteElementCollection *l2_coll
            = new L2_FECollection(deg, ndim);
    FiniteElementSpace *W_space
            = new FiniteElementSpace(mesh.get(), l2_coll);

    // Divergence operator
    // \int_{\Omega} div(u_h) q_h d_{\Omega}
    MixedBilinearForm *div_form
            = new MixedBilinearForm(R_space,
                                    W_space);
    ConstantCoefficient one(-1.0);
    div_form->AddDomainIntegrator
            (new VectorFEDivergenceIntegrator(one));
    div_form->Assemble();
    div_form->Finalize();
    SparseMatrix *div = div_form->LoseMat();
    delete div_form;

    // velocity
    std::shared_ptr <GridFunction> v
            = std::make_shared<GridFunction>(R_space);
    Sampler<Uniform> unifSampler(10, 1);
    Vector omegas = unifSampler.generate_one_sample();
    IncompNSInitialVelocityCoeff v0_coeff(testCase);
    testCase->set_perturbations(omegas);
    v0_coeff.SetTime(0);
    v->ProjectCoefficient(v0_coeff);

    // oberver
    std::shared_ptr<IncompNSObserver> observer
            = std::make_shared<IncompNSObserver>
            (config, lx);
    //(*observer)(v);

    double div_old = measure_divergence(div, v.get());
    std::cout << "Weak divergence before cleaning: "
              << div_old << std::endl;

    // make divergence free
    DivergenceFreeVelocity divFreeVel (config, mesh);
    divFreeVel (v.get());
    //(*observer)(v);

    double div_new = measure_divergence(div, v.get());
    std::cout << "Weak divergence after cleaning: "
              << div_new << std::endl;

    double TOL=1E-4;
    ASSERT_LE(div_new, TOL);
}

// End of file


#include <scitbx/random/boost_python/random.h>

#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>

namespace scitbx { namespace random { namespace boost_python {

namespace {

  struct uniform
  {
    typedef boost::uniform_real<double> wt;

    static std::string name() { return "uniform"; }

    static void wrap_specific(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass
        .def(init<double, double>((arg("min"), arg("max"))))
        .add_property("min", &wt::min)
        .add_property("min", &wt::min)
      ;
    }
  };

  struct normal
  {
    typedef boost::normal_distribution<double> wt;

    static std::string name() { return "normal"; }

    static void wrap_specific(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass
        .def(init<double, double>((arg("mean") =0.,
                                   arg("sigma")=1.)))
        .add_property("mean", &wt::mean)
        .add_property("sigma", &wt::sigma)
        ;
    }
  };

  struct bernoulli
  {
    typedef boost::bernoulli_distribution<double> wt;

    static std::string name() { return "bernoulli"; }

    static void wrap_specific(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass
        .def(init<double>())
        .add_property("p", &wt::p)
        ;
    }
  };

  struct gamma
  {
    typedef boost::gamma_distribution<double> wt;

    static std::string name() { return "gamma"; }

    static void wrap_specific(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass
        .def(init<double, double>((arg("alpha")=1.0,
                                   arg("beta")=1.0)))
        .add_property("alpha", &wt::alpha)
        .add_property("beta", &wt::beta)
        ;
    }
  };

  struct poisson
  {
    typedef boost::poisson_distribution<int,double> wt;

    static std::string name() { return "poisson"; }

    static void wrap_specific(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass
        .def(init<double>((arg("mean")=1.0)))
        .add_property("mean", &wt::mean)
        ;
    }
  };

} // namespace <anonymous>

  void wrap_random()
  {
    wrap_distribution_and_variate<uniform>();
    wrap_distribution_and_variate<normal>();
    wrap_distribution_and_variate<bernoulli>();
    wrap_distribution_and_variate<gamma>();
    wrap_distribution_and_variate<poisson>();
  }

}}} // namespace scitbx::random::boost_python


#include <boost/test/unit_test.hpp>

#include <scorum/protocol/asset.hpp>
#include <scorum/protocol/odds.hpp>

#include <scorum/chain/betting/betting_math.hpp>

#include "defines.hpp"

namespace betting_math_tests {

using namespace scorum::protocol;
using namespace scorum::chain;

BOOST_AUTO_TEST_SUITE(betting_math_tests)

SCORUM_TEST_CASE(extra_large_coefficient_big_gain_mismatch_test)
{
    const asset bet1_stake = ASSET_SCR(10000);
    const asset bet2_stake = ASSET_SCR(90'000'000);

    const odds bet1_odds(10000, 1);
    const odds bet2_odds(10000, 9999);

    const auto matched = calculate_matched_stake(bet1_stake, bet2_stake, bet1_odds, bet2_odds);

    BOOST_CHECK_EQUAL(matched.bet1_matched.amount, 9000);
    BOOST_CHECK_EQUAL(matched.bet2_matched.amount, bet2_stake.amount);
}

SCORUM_TEST_CASE(more_than_matched_check) // to catch and log exceptions in calculate_matched_stake
{
    const asset stake_my = ASSET_SCR(1e+9);
    const asset stake_other = ASSET_SCR(2e+9);

    const odds odds_my = odds(10, 1);
    const odds odds_other = odds(10, 9);

    BOOST_CHECK_EQUAL(stake_my * odds_my, ASSET_SCR(10'000'000'000));
    BOOST_CHECK_EQUAL(stake_other * odds_other, ASSET_SCR(2'222'222'222));

    BOOST_CHECK_EQUAL(odds_my.simplified(), odds_other.inverted());
    BOOST_CHECK_EQUAL(odds_other.simplified(), odds_my.inverted());

    auto matched = calculate_matched_stake(stake_my, stake_other, odds_my, odds_other);

    BOOST_REQUIRE_EQUAL(matched.bet1_matched.amount.value, 222'222'222);
}

SCORUM_TEST_CASE(less_than_matched_check)
{
    const asset stake_my = ASSET_SCR(0.1e+9);
    const asset stake_other = ASSET_SCR(2e+9);

    const odds odds_my = odds(10, 1);
    const odds odds_other = odds(10, 9);

    BOOST_CHECK_EQUAL(stake_my * odds_my, ASSET_SCR(1'000'000'000));
    BOOST_CHECK_EQUAL(stake_other * odds_other, ASSET_SCR(2'222'222'222));

    auto matched = calculate_matched_stake(stake_my, stake_other, odds_my, odds_other);

    BOOST_REQUIRE_EQUAL(matched.bet2_matched.amount.value, 900'000'000);
}

SCORUM_TEST_CASE(equal_matched_check)
{
    const asset stake_my = ASSET_SCR(1e+9);
    const asset stake_other = ASSET_SCR(9e+9);

    const odds odds_my = odds(10, 1);
    const odds odds_other = odds(10, 9);

    BOOST_CHECK_EQUAL(stake_my * odds_my, stake_other * odds_other);

    auto matched = calculate_matched_stake(stake_my, stake_other, odds_my, odds_other);

    BOOST_REQUIRE_EQUAL(matched.bet1_matched, stake_my);

    matched = calculate_matched_stake(stake_other, stake_my, odds_other, odds_my);

    BOOST_REQUIRE_EQUAL(matched.bet1_matched, stake_other);
}

SCORUM_TEST_CASE(calculate_matched_stake_negative_check)
{
    BOOST_CHECK_THROW(calculate_matched_stake(ASSET_SCR(1e+9), ASSET_SP(9e+9), odds(10, 1), odds(10, 9)),
                      fc::assert_exception);
    BOOST_CHECK_THROW(calculate_matched_stake(ASSET_SP(1e+9), ASSET_SCR(9e+9), odds(10, 1), odds(10, 9)),
                      fc::assert_exception);
    BOOST_CHECK_THROW(calculate_matched_stake(ASSET_SP(1e+9), ASSET_SP(9e+9), odds(10, 1), odds(10, 9)),
                      fc::assert_exception);

    BOOST_CHECK_THROW(calculate_matched_stake(ASSET_SCR(1e+9), ASSET_SCR(9e+9), odds(10, 1), odds(10, 5)),
                      fc::assert_exception);
    BOOST_CHECK_THROW(calculate_matched_stake(ASSET_SCR(1e+9), ASSET_SCR(9e+9), odds(10, 5), odds(10, 9)),
                      fc::assert_exception);
}

SCORUM_TEST_CASE(calculate_gain_positive_check)
{
    auto potential_profit = calculate_gain(ASSET_SCR(1e+9), odds(10, 1));

    BOOST_CHECK_EQUAL(potential_profit, ASSET_SCR(9e+9));
}

SCORUM_TEST_CASE(min_bet_min_odds)
{
    const asset bet_stake = SCORUM_MIN_BET_STAKE;
    const odds bet_odds = SCORUM_MIN_ODDS;

    auto r1 = bet_stake * bet_odds;

    BOOST_CHECK_EQUAL(1'001'000u, r1.amount);
}

SCORUM_TEST_CASE(min_bet_max_odds)
{
    const asset bet_stake = SCORUM_MIN_BET_STAKE;
    const odds bet_odds = SCORUM_MIN_ODDS.inverted();

    auto r1 = bet_stake * bet_odds;

    BOOST_CHECK_EQUAL(1'001'000'000u, r1.amount);
}

SCORUM_TEST_CASE(calculate_potential_result)
{
    const asset bet_stake = asset::from_string("1000000.000000000 SCR");
    const odds bet_odds = SCORUM_MIN_ODDS.inverted();

    auto result = bet_stake * bet_odds;

    BOOST_CHECK_EQUAL("1001000000.000000000 SCR", result.to_string());
}

SCORUM_TEST_CASE(calc_max_bet_stake)
{
    asset bet_stake(share_type::max(), SCORUM_SYMBOL);

    BOOST_CHECK_EQUAL("9223372036.854775807 SCR", bet_stake.to_string());

    const odds bet_odds = SCORUM_MIN_ODDS.inverted();

    auto result = bet_stake * bet_odds.base().coup();

    BOOST_CHECK_EQUAL(SCORUM_MAX_BET_STAKE, result);
}

SCORUM_TEST_CASE(calc_potential_result_for_max_stake_max_odds)
{
    asset bet_stake = SCORUM_MAX_BET_STAKE;
    const odds bet_odds = SCORUM_MIN_ODDS.inverted();

    auto result = bet_stake * bet_odds;

    BOOST_CHECK_EQUAL("9223372036.854775800 SCR", result.to_string());
}

SCORUM_TEST_CASE(calculate_matched_stake_for_max_stake_max_odds)
{
    asset bet1_stake = SCORUM_MAX_BET_STAKE;
    const odds bet1_odds = SCORUM_MIN_ODDS.inverted();

    asset bet2_stake = bet1_stake * bet1_odds - bet1_stake;
    odds bet2_odds = SCORUM_MIN_ODDS;

    std::cout << bet2_stake.to_string() << std::endl;

    auto matched = calculate_matched_stake(bet1_stake, bet2_stake, bet1_odds, bet2_odds);

    BOOST_CHECK_EQUAL(bet1_stake, matched.bet1_matched);
    BOOST_CHECK_EQUAL(bet2_stake, matched.bet2_matched);
}

SCORUM_TEST_CASE(calculate_matched_stake_throw_exception_when_potential_result_is_to_big)
{
    asset bet1_stake = SCORUM_MAX_BET_STAKE + 10;
    const odds bet1_odds = SCORUM_MIN_ODDS.inverted();

    asset bet2_stake = asset::from_string("9214157878.975800000 SCR");
    odds bet2_odds = SCORUM_MIN_ODDS;

    SCORUM_CHECK_EXCEPTION(calculate_matched_stake(bet1_stake, bet2_stake, bet1_odds, bet2_odds),
                           fc::underflow_exception, "");
}

BOOST_AUTO_TEST_SUITE_END()
}


#include <ros/ros.h>
#include <iostream>
#include <tagloc/algorithms.h>
#include <tagloc/tracking.h>
#include <Eigen/Dense>
#include <math.h>
#include <Eigen/Geometry>
#include <vector>

double Dei = 100;
double sensor_var=pow((M_PI/12),2);
using namespace std;
double zof(Eigen::MatrixXd T, Eigen::MatrixXd R){
	double nz = (rand()%100-50)/(100.0) * M_PI/(16);
	return atan2(T(1)-R(1),T(0)-R(0))+nz;
}
//useful stuff
double get_radius(Eigen::MatrixXd P){
	assert(P.rows()==P.cols());
	Eigen::JacobiSVD<Eigen::MatrixXd> _svdA;
	_svdA.compute(P);
	return sqrt(_svdA.singularValues()(0));
}
int main(int argc, char** argv){
	if (argc>1){
		Dei = atof(argv[1]);
		sensor_var = atof(argv[2]);
	}
	std::vector<double> px;
	std::vector<double> py;
	std::vector<double> Z;
	std::vector<double> R;
	Eigen::MatrixXd T(2,1);
	Eigen::MatrixXd TT(2,1);
	T<< 0,0;
	TT<< 0,0;
	Eigen::MatrixXd P = Eigen::MatrixXd::Identity(2,2) * 900.0;
	Eigen::MatrixXd r1(2,1),r2(2,1);
	r1 << 200,20;
	r2 << 200,-20;
	int i=0;
	while(get_radius(P)>sqrt(Dei)){
		cout<<"\t\t\ti:"<<i++<<endl;
		px.push_back(r1(0));
		px.push_back(r2(0));
		py.push_back(r1(1));
		py.push_back(r2(1));
		R.push_back((sensor_var));
		R.push_back((sensor_var));
		Z.push_back(zof(TT,r1));
		Z.push_back(zof(TT,r2));
		double ti[2];
		double c[4];
		RSN::BOT::IWLS_2D(px.size(),px,py,Z,R,ti,c,false);
		T<<ti[0],ti[1];
		P<<c[0],c[1],c[2],c[3];
		//P=Eigen::MatrixXd::Identity(2,2)* 900.0;
		double rad = get_radius(P);
		cout<<"--------------------"<<endl;
		cout<<T<<endl;
		cout<<P<<endl;
		cout<<"e1:"<<rad<<endl;
		cout<<"--------------------"<<endl;
		RSN::onestep_circle(r1,r2,T,rad,(sensor_var),Dei);
		Eigen::MatrixXd rot(2,2);
		double th =0; //M_PI/8;
		rot(0,0)=cos(th);
		rot(0,1)=-sin(th);
		rot(1,0)=sin(th);
		rot(1,1)=cos(th);
		r1 = rot * r1;
		r2 = rot * r2;
	}
	cout<<"Got there in "<<i<<endl;
	cout<<T<<endl;

	return EXIT_SUCCESS;
}


#include <Eigen/Core>
#include <Eigen/Dense>

class LKF{
public:
    LKF();
    LKF(float init_pose_x, float init_pose_y);
    LKF(Eigen::Matrix2f _H_k, Eigen::Matrix2f _Q_k, Eigen::Matrix2f _R_k, Eigen::Matrix2f _P_k_prev);
    void set_init_pose(float init_pose_x, float init_pose_y);
    Eigen::Vector2f run(float theta_t, Eigen::Vector2f u_k, Eigen::Vector2f z_k);
    Eigen::Vector2f run_without_update(float theta_t, Eigen::Vector2f u_k);
    void restore();
    // u_k: control vector, z_k: observation vector
private:
    Eigen::Matrix2f H_k; // Scale Matrix
    Eigen::Matrix2f Q_k; // Prediction Noise
    Eigen::Matrix2f R_k; // Observation Noise
    Eigen::Vector2f x_hat_k_prev; // Previous Value
    Eigen::Matrix2f P_k_prev; // Previous Prediction Covariance Matrix
    Eigen::Vector2f x_hat_k_prev_prev;
    Eigen::Matrix2f P_k_prev_prev;
};


#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/range/algorithm.hpp>
#include <boost/integer/common_factor_rt.hpp>

using namespace std;

int N;
int main() {
    while (cin >> N) {
        int a[N], box[N], M = 0;
        for (int i = 0; i < N; i++) {
            cin >> a[i];
        }
        for (int i = N; i > 0; i--) {
            int ball = a[i - 1];
            for (int j = 2; i * j <= N; j++) {
                // cout << "DEBUG: i = " << i << ", " << (i * j - 1) << ", " << box[i * j - 1] << endl;
                ball += box[i * j - 1];
            }
            // cout << "DEBUG: " << i << " " << ball << endl;
            if (ball % 2 == 1) {
                box[i - 1] = 1;
                M++;
            } else {
                box[i - 1] = 0;
            }
        }
        cout << M << endl;
        if (M != 0) {
            for (int i = 0; i < N; i++) {
                if (box[i] == 1) {
                    cout << (i + 1) << " ";
                }
            }
            cout << endl;
        }
    }
    return 0;
}


#ifndef USE_CUDA

#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <utility/Logging.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Dense>

#include <array>

namespace Engine
{
	namespace Manifoldmath
	{
        scalar norm(const vectorfield & vf)
        {
            scalar x = Vectormath::dot(vf, vf);
            return std::sqrt(x);
        }

        void normalize(vectorfield & vf)
        {
            scalar x = 1.0/norm(vf);
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf.size(); ++i)
                vf[i] *= x;
        }

        void project_parallel(vectorfield & vf1, const vectorfield & vf2)
        {
            vectorfield vf3 = vf1;
            project_orthogonal(vf3, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf1.size(); ++i)
                vf1[i] -= vf3[i];
        }

        void project_orthogonal(vectorfield & vf1, const vectorfield & vf2)
        {
            scalar x = Vectormath::dot(vf1, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i=0; i<vf1.size(); ++i)
                vf1[i] -= x*vf2[i];
        }

        void invert_parallel(vectorfield & vf1, const vectorfield & vf2)
        {
            scalar x = Vectormath::dot(vf1, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i=0; i<vf1.size(); ++i)
                vf1[i] -= 2*x*vf2[i];
        }

        void invert_orthogonal(vectorfield & vf1, const vectorfield & vf2)
        {
            vectorfield vf3 = vf1;
            project_orthogonal(vf3, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf1.size(); ++i)
                vf1[i] -= 2 * vf3[i];
        }

        void project_tangential(vectorfield & vf1, const vectorfield & vf2)
		{
            #pragma omp parallel for
			for (unsigned int i = 0; i < vf1.size(); ++i)
				vf1[i] -= vf1[i].dot(vf2[i]) * vf2[i];
		}

		scalar dist_greatcircle(const Vector3 & v1, const Vector3 & v2)
		{
			scalar r = v1.dot(v2);

			// Prevent NaNs from occurring
			r = std::fmax(-1.0, std::fmin(1.0, r));

			// Greatcircle distance
			return std::acos(r);
		}

		scalar dist_geodesic(const vectorfield & v1, const vectorfield & v2)
		{
			scalar dist = 0;
            #pragma omp parallel for reduction(+:dist)
			for (unsigned int i = 0; i < v1.size(); ++i)
				dist += pow(dist_greatcircle(v1[i], v2[i]), 2);
			return sqrt(dist);
		}

		/*
		Calculates the 'tangent' vectors, i.e.in crudest approximation the difference between an image and the neighbouring
		*/
		void Tangents(std::vector<std::shared_ptr<vectorfield>> configurations, const std::vector<scalar> & energies, std::vector<vectorfield> & tangents)
		{
			int noi = configurations.size();
			int nos = (*configurations[0]).size();

			for (int idx_img = 0; idx_img < noi; ++idx_img)
			{
				auto& image = *configurations[idx_img];

				// First Image
				if (idx_img == 0)
				{
					auto& image_plus = *configurations[idx_img + 1];
					Vectormath::set_c_a( 1, image_plus, tangents[idx_img]);
					Vectormath::add_c_a(-1, image,      tangents[idx_img]);
				}
				// Last Image
				else if (idx_img == noi - 1)
				{
					auto& image_minus = *configurations[idx_img - 1];
					Vectormath::set_c_a( 1, image,       tangents[idx_img]);
					Vectormath::add_c_a(-1, image_minus, tangents[idx_img]);
				}
				// Images Inbetween
				else
				{
					auto& image_plus  = *configurations[idx_img + 1];
					auto& image_minus = *configurations[idx_img - 1];

					// Energies
					scalar E_mid = 0, E_plus = 0, E_minus = 0;
					E_mid   = energies[idx_img];
					E_plus  = energies[idx_img + 1];
					E_minus = energies[idx_img - 1];

					// Vectors to neighbouring images
					vectorfield t_plus(nos), t_minus(nos);

					Vectormath::set_c_a( 1, image_plus, t_plus);
					Vectormath::add_c_a(-1, image,      t_plus);

					Vectormath::set_c_a( 1, image,       t_minus);
					Vectormath::add_c_a(-1, image_minus, t_minus);

					// Near maximum or minimum
					if ((E_plus < E_mid && E_mid > E_minus) || (E_plus > E_mid && E_mid < E_minus))
					{
						// Get a smooth transition between forward and backward tangent
						scalar E_max = std::max(std::abs(E_plus - E_mid), std::abs(E_minus - E_mid));
						scalar E_min = std::min(std::abs(E_plus - E_mid), std::abs(E_minus - E_mid));

						if (E_plus > E_minus)
						{
							Vectormath::set_c_a(E_max, t_plus,  tangents[idx_img]);
							Vectormath::add_c_a(E_min, t_minus, tangents[idx_img]);
						}
						else
						{
							Vectormath::set_c_a(E_min, t_plus,  tangents[idx_img]);
							Vectormath::add_c_a(E_max, t_minus, tangents[idx_img]);
						}
					}
					// Rising slope
					else if (E_plus > E_mid && E_mid > E_minus)
					{
						Vectormath::set_c_a(1, t_plus,  tangents[idx_img]);
					}
					// Falling slope
					else if (E_plus < E_mid && E_mid < E_minus)
					{
						Vectormath::set_c_a(1, t_minus,  tangents[idx_img]);
						//tangents = t_minus;
						for (int i = 0; i < nos; ++i)
						{
							tangents[idx_img][i] = t_minus[i];
						}
					}
					// No slope(constant energy)
					else
					{
						Vectormath::set_c_a(1, t_plus,  tangents[idx_img]);
						Vectormath::add_c_a(1, t_minus, tangents[idx_img]);
					}
				}

				// Project tangents into tangent planes of spin vectors to make them actual tangents
        		project_tangential(tangents[idx_img], image);

				// Normalise in 3N - dimensional space
				Manifoldmath::normalize(tangents[idx_img]);

			}// end for idx_img
		}// end Tangents
    }
}

#endif


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <cstdint>
#include <vector>
using gint = std::int64_t;
using Coordinate = std::complex<gint>;
enum Command {
  TurnRight,
  TurnLeft
};
class Car
{
  std::complex<gint> pos_{ 0, 0 };
  std::complex<gint> direction_{ 1, 0 };
  std::complex<gint> turn_[2]{
    { 0, -1 },
    { 0, 1 }
  };

public:
  constexpr gint distance() const noexcept
  {
    return std::abs(pos_.real()) + std::abs(pos_.imag());
  }

  void process(Command command, gint value) noexcept
  {
    direction_ *= turn_[command];
    pos_ += direction_ * value;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    char c;
    int forward;
    Car car;
    Command com[256];
    com['L'] = Command::TurnLeft;
    com['R'] = Command::TurnRight;
    while (ifs >> c >> forward) {
      car.process(com[c], forward);
    }
    fmt::print("d:{}\n", car.distance());
  }
}


//----------------------------------*-C++-*----------------------------------//
/**
 *  @file   CLP.cc
 *  @brief  CLP
 *  @author Jeremy Roberts
 *  @date   Jan 8, 2013
 */
//---------------------------------------------------------------------------//

#include "CLP.hh"
#ifdef DETRAN_ENABLE_BOOST
#include <boost/math/special_functions/legendre.hpp>
#endif

namespace detran_orthog
{

//---------------------------------------------------------------------------//
CLP::CLP(const size_t   order,
         const vec_dbl &x,
         const vec_dbl &qw,
         const double   x_0,
         const double   x_1)
  : ContinuousOrthogonalBasis(order, x, qw)
{
#ifndef DETRAN_ENABLE_BOOST
  THROW("CLP needs boost to be enabled.");
#else
  Require(x_1 > x_0);

  // Allocate the basis matrix
  d_basis = new callow::MatrixDense(d_order + 1, d_size, 0.0);

  // Allocate the normalization array
  d_a = Vector::Create(d_order + 1, 0.0);

  // The weights are just the qw's.
  double L = x_1 - x_0;
  for (size_t i = 0; i < d_w->size(); ++i)
  {
    d_x[i] =  2.0*(d_x[i] - x_0)/L - 1.0;
    (*d_w)[i] = d_qw[i];
  }

  // Build the basis
  for (size_t l = 0; l <= d_order; ++l)
  {
    for (size_t i = 0; i < d_size; ++i)
    {
      (*d_basis)(l, i) = boost::math::legendre_p(l, d_x[i]);
    }
    // Inverse of normalization coefficient.
    (*d_a)[l] = (2.0 * l + 1.0) / 2.0;
  }
  d_orthonormal = true;
  compute_a();
#endif
}

} // end namespace detran_orthog

//---------------------------------------------------------------------------//
//              end of file CLP.cc
//---------------------------------------------------------------------------//


#include <iostream>
#include <vector>
#include <string>
#include "jefflib.h"
#include <boost/tokenizer.hpp>
#include <cmath>
#include <map>

using namespace std;
using namespace boost;

struct coord_t {
    int x;
    int y;
};

struct loc_t {
    int x;
    int y;
    int closestDist = INT_MAX;
    int closestCoord = -1;
    int closestCoordCount = 0;
};

int main()
{
    vector<string> vect;
    if(GetStringInput(vect)){
        cout << "Got data!" << endl;
        cout << endl;
    }
    else {
        cout << "Failed to read input :( " << cout;
        return -1;
    }

    // Create the list of coords, making note of bounds
    vector<coord_t> coords;
    int minX = INT_MAX, minY = INT_MAX;
    int maxX = 0, maxY = 0;
    for (const string line : vect)
    {
        char_separator<char> sep(",");
        tokenizer< char_separator<char> > tokens(line, sep);
        // x y
        vector<string> s_tmp(tokens.begin(), tokens.end());
        coord_t tempCoord;
        tempCoord.x = stoi(s_tmp[0]);
        tempCoord.y = stoi(s_tmp[1]);
        coords.push_back(tempCoord);

        if(tempCoord.x < minX) minX = tempCoord.x;
        if(tempCoord.x > maxX) maxX = tempCoord.x;
        if(tempCoord.y < minY) minY = tempCoord.y;
        if(tempCoord.y > maxY) maxY = tempCoord.y;
    }

    cout << "Min x: " << minX << ", Max x: " << maxX << ", Min y: " << minY << ", Max y: " << maxY << endl;

    // Hack(?) to try and identify which coord are bounded vs. unbounded...
    minX = -2000;
    minY = -2000;
    maxX = 2000;
    maxY = 2000;

    // Let's build a (bounded) list of locations of interest
    vector<loc_t> locs;
    for(int x = minX; x <= maxX; x++){
        for(int y = minY; y <= maxY; y++){
            loc_t tempLoc;
            tempLoc.x = x;
            tempLoc.y = y;
            locs.push_back(tempLoc);
        }
    }
    cout << "Number of locations: " << locs.size() << endl;
    // For each location, let's find the closest coord(s)
    for(auto  &location : locs){
        for(int i = 0; i < coords.size(); i++){
            int distance = abs(location.x - coords[i].x) + abs(location.y - coords[i].y);
            if(distance == location.closestDist){
                location.closestCoord = -1;
                location.closestCoordCount++;
            }
            else if(distance < location.closestDist){
                location.closestDist = distance;
                location.closestCoord = i;
                location.closestCoordCount = 1;
            }
        }
    }

    // Finally, let's determine which coord appears most frequently (i.e. has the largest area)
    map<int, int> m; // coordId, count map
    for(auto &location : locs){
        // Need to skip over the edge/infinite coords
        if(location.x == maxX || location.x == minX || location.y == maxY || location.y == minY) continue;

        if(location.closestCoordCount == 1){
            m[location.closestCoord] += 1;
        }
        //cout << "Loc: " << location.x << "," << location.y << ": closestCoord = " << location.closestCoord << ", ClosestCoordCount = " << location.closestCoordCount << endl;
    }

    int maxArea = 0;
    for (const auto &p : m) {
        cout << p.first << ", " << p.second << endl;
        if(p.second > maxArea) maxArea = p.second;
    }
    cout << "Max area: " << maxArea << endl;
}


#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <sensor_msgs/Imu.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>

geometry_msgs::PoseStamped pose;
mavros_msgs::State current_state;
Eigen::Quaternionf q;
Eigen::Quaternionf q_2;
Eigen::MatrixXf R_mat(3,3);
Eigen::MatrixXf imuacc(3,1);
geometry_msgs::Vector3Stamped ang;
geometry_msgs::Vector3Stamped acc_;
geometry_msgs::Vector3Stamped acc_ang;
ros::Publisher euler ;
ros::Publisher body_acc;
ros::Publisher acc_1;
ros::Publisher acc_2;
ros::Publisher veloc;

double a,b;
double theta=0.0;
double count_r=0.0;
double wn;

void imu_cb(const sensor_msgs::Imu::ConstPtr& msg)
{
    //conerts the acceleration from body frame to earth frame(NED)
    q = Eigen::Quaternionf(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
    R_mat= q.toRotationMatrix();
    imuacc << msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z;
    imuacc= R_mat*imuacc ;
    acc_.header.stamp = ros::Time::now();
    acc_.vector.x = imuacc(0,0);
    acc_.vector.y = imuacc(1,0);
    acc_.vector.z = imuacc(2,0)-9.81;
    acc_1.publish(acc_);

   /* Eigen::MatrixXf rpy(3,1);
    rpy = q.toRotationMatrix().eulerAngles(0, 1, 2);
    acc_ang.header.stamp = ros::Time::now();
    acc_ang.vector.x = rpy(0,0)*(180.0/3.14159265358979);
    acc_ang.vector.y = rpy(1,0)*(180.0/3.14159265358979);
    acc_ang.vector.z = rpy(2,0)*(180.0/3.14159265358979);

    body_acc.publish(acc_ang);*/

}

double prev_x,prev_y,prev_z,dt_1,vel_last_time;
geometry_msgs::Vector3Stamped vic_vel;
int count_vel=0;

double prev_vx,prev_vy,prev_vz,dt_2,acc_last_time;
geometry_msgs::Vector3Stamped vic_acc;
int count_acc=0;

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe("/mavros/state", 10, state_cb);
    ros::Subscriber Imu=nh.subscribe("/mavros/imu/data",10,imu_cb);

    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("/mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("/mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("/mavros/set_mode");

    acc_1 = nh.advertise<geometry_msgs::Vector3Stamped>("imu_acc", 10);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(5.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    //geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 1.5;
    pose.pose.position.z = 1.5;

    pose.pose.orientation.x = 0.0;
    pose.pose.orientation.y = 0.0;
    pose.pose.orientation.z = 0.0;
    pose.pose.orientation.w = 1.0;
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    double pos_x = 0.0;
    double pos_y = 1.5;
    int mode_sp=0;
    while(ros::ok()){

  //  if( current_state.mode == "OFFBOARD" )
    nh.getParam("offboard/mode_sp", mode_sp);

    if (mode_sp == 0)
    {
        nh.getParam("offboard/pos_x", pos_x);
        nh.getParam("offboard/pos_y", pos_y);
        pose.pose.position.x = pos_x;
        pose.pose.position.y = pos_y;
        pose.pose.position.z = 1.5;
    }

    if(mode_sp == 1)
    {

        nh.getParam("offboard/elip_a", a);
        nh.getParam("offboard/elip_b", b);
        nh.getParam("offboard/change_rate", wn);
        theta = wn*count_r;

        pose.pose.position.x = a*sin(theta);
        pose.pose.position.y = b*cos(theta);
        pose.pose.position.z = 1.5;

        count_r = count_r + 1.0;
        if(theta > 360)
            theta = 0 ;
    }

	    pose.pose.orientation.x = 0.0;
    	pose.pose.orientation.y = 0.0;
    	pose.pose.orientation.z = 0.0;
    	pose.pose.orientation.w = 1.0;

        local_pos_pub.publish(pose);

    ros::spinOnce();
    rate.sleep();

//
}

return 0;
}


// Copyright (c) 2017-2019 The Multiverse developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/test/unit_test.hpp>

#include "test_fnfn.h"
#include "mpvss.h"
#include "mpinterpolation.h"

using curve25519::Print32;

BOOST_FIXTURE_TEST_SUITE(mpvss_tests, BasicUtfSetup)

void RandGeneretor(uint8_t* p)
{
    for (int i = 0; i < 32; i++)
    {
        *p++ = rand();
    }
}

void KeyGenerator(uint256& priv, uint256& pub)
{
    uint8_t md32[32];

    RandGeneretor(md32);
    md32[31] &= 0x0F;
    memcpy(priv.begin(), md32, 32);

    CEdwards25519 P;
    P.Generate(priv);
    P.Pack(pub.begin());
}

BOOST_AUTO_TEST_CASE( fp25519 )
{
    srand(time(0));
    uint8_t md32[32];

    // add, minus
    for (int i = 0; i < 10; i++)
    {
        RandGeneretor(md32);
        CFP25519 add1(md32);
        RandGeneretor(md32);
        CFP25519 add2(md32);
        CFP25519 fpSum = add1 + add2;
        BOOST_CHECK( add1 == fpSum - add2 );
        BOOST_CHECK( add2 == fpSum - add1 );
    }

    // multiply, divide
    for (int i = 0; i < 10; i++)
    {
        RandGeneretor(md32);
        CFP25519 mul1(md32);
        RandGeneretor(md32);
        CFP25519 mul2(md32);
        CFP25519 fpProduct = mul1 * mul2;
        BOOST_CHECK( mul1 == fpProduct / mul2 );
        BOOST_CHECK( mul2 == fpProduct / mul1 );
    }

    // inverse
    for (int i = 0; i < 10; i++)
    {
        RandGeneretor(md32);
        CFP25519 fp(md32);
        CFP25519 fpInverse = fp.Inverse();
        BOOST_CHECK( CFP25519(1) == fp * fpInverse );
    }

    // sqrt square
    for (int i = 0; i < 10; i++)
    {
        RandGeneretor(md32);
        CFP25519 fp1(md32);
        CFP25519 fp2(md32);
        fp2 = fp2.Square().Sqrt();
        BOOST_CHECK( fp2.Square() == fp1.Square() );
    }
}

BOOST_AUTO_TEST_CASE( sc25519 )
{
    srand(time(0));
    uint8_t md32[32];

    // add, minus
    for (int i = 0; i < 10; i++)
    {
        RandGeneretor(md32);
        CSC25519 add1(md32);
        RandGeneretor(md32);
        CSC25519 add2(md32);
        CSC25519 scSum = add1 + add2;
        BOOST_CHECK( add1 == scSum - add2 );
        BOOST_CHECK( add2 == scSum - add1 );
    }

    // multiply
    for (int i = 0; i < 10; i++)
    {
        RandGeneretor(md32);
        CSC25519 sc(md32);
        RandGeneretor(md32);
        uint32_t n = 10;
        CSC25519 scProduct = sc * n;
        for (int i = 0; i < n - 1; i++)
        {
            scProduct -= sc;
        }
        BOOST_CHECK( scProduct == sc );
    }

    // negative
    for (int i = 0; i < 10; i++)
    {
        RandGeneretor(md32);
        CSC25519 sc(md32);
        CSC25519 scNegative1 = -sc;
        CSC25519 scNegative2(sc);
        scNegative2.Negative();
        BOOST_CHECK( scNegative1 == scNegative2 );
        BOOST_CHECK( CSC25519() == sc + scNegative1 );
    }
}

BOOST_AUTO_TEST_CASE( ed25519 )
{
    srand(time(0));
    uint8_t md32[32];

    // sign, verify
    for (int i = 0; i < 10; i++)
    {
        uint256 priv1, pub1;
        uint256 priv2, pub2;
        KeyGenerator(priv1, pub1);
        KeyGenerator(priv2, pub2);

        RandGeneretor(md32);
        CSC25519 hash(md32);

        CSC25519 sign = CSC25519(priv1.begin()) + CSC25519(priv2.begin()) * hash;

        CEdwards25519 P,R,S;
        P.Unpack(pub1.begin());
        R.Unpack(pub2.begin());
        S.Generate(sign);

        BOOST_CHECK( S == (P + R.ScalarMult(hash)) );
    }
}

BOOST_AUTO_TEST_CASE( interpolation )
{
    srand(time(0));
    uint8_t md32[32];

    // lagrange, newton
    for (int i = 0; i < 1; i++)
    {
        std::vector<uint32_t> vX;
        for (int i = 1; i < 51; i++)
        {
            vX.push_back(i);
        }

        std::vector<std::pair<uint32_t,uint256> > vShare;
        for (int i = 0; i < 26; i++)
        {
            int index = rand() % vX.size();
            uint32_t x = vX[index];
            vX.erase(vX.begin() + index);

            RandGeneretor(md32);
            uint256 y((uint64_t*)md32);
            vShare.push_back(std::make_pair(x, y));
        }

        BOOST_CHECK( MPLagrange(vShare) == MPNewton(vShare) );
    }
}

BOOST_AUTO_TEST_CASE( mpvss )
{
    srand(time(0));
    for (size_t count = 41; count <= 50; count++)
    {
        uint256 nInitValue;
        std::vector<uint256> vID;
        std::map<uint256,CMPSecretShare> mapSS;
        std::vector<CMPSealedBox> vSBox;
        std::vector<CMPCandidate> vCandidate;

        CMPSecretShare ssWitness;

        boost::posix_time::ptime t0;
        std::cout << "Test mpvss begin: count " << count << "\n{\n";
        vID.resize(count); vSBox.resize(count); vCandidate.resize(count);
        //Setup
        t0 = boost::posix_time::microsec_clock::universal_time();
        for (int i = 0;i < count;i++)
        {
            vID[i] = uint256(i + 1);
            mapSS[vID[i]] = CMPSecretShare(vID[i]);

            mapSS[vID[i]].Setup(count + 1,vSBox[i]);
            vCandidate[i] = CMPCandidate(vID[i],1,vSBox[i]);

            nInitValue = nInitValue ^ mapSS[vID[i]].myBox.vCoeff[0];
        }
        std::cout << "\tSetup : " << ((boost::posix_time::microsec_clock::universal_time() - t0).ticks() / count) <<"\n";
        std::cout << "\tInit value = " << nInitValue.GetHex() << "\n";
        {
            CMPSealedBox box;
            ssWitness.Setup(count + 1,box);
        }

        //Enroll
        t0 = boost::posix_time::microsec_clock::universal_time();
        for (int i = 0;i < count;i++)
        {
            mapSS[vID[i]].Enroll(vCandidate);
        }
        std::cout << "\tEnroll : " << ((boost::posix_time::microsec_clock::universal_time() - t0).ticks() / count) <<"\n";
        ssWitness.Enroll(vCandidate);

        // Distribute
        t0 = boost::posix_time::microsec_clock::universal_time();
        for (int i = 0;i < count;i++)
        {
            std::map<uint256,std::vector<uint256> > mapShare;
            mapSS[vID[i]].Distribute(mapShare);
            for (int j = 0;j < count;j++)
            {
                if (i != j)
                {
                    mapSS[vID[j]].Accept(vID[i],mapShare[vID[j]]);
                }
            }
        }
        std::cout << "\tDistribute : " << ((boost::posix_time::microsec_clock::universal_time() - t0).ticks() / count) <<"\n";

        // Publish
        t0 = boost::posix_time::microsec_clock::universal_time();
        bool fComplete;
        for (int i = 0;i < count;i++)
        {
            std::map<uint256,std::vector<uint256> > mapShare;
            mapSS[vID[i]].Publish(mapShare);
            for (int j = 0;j < count;j++)
            {
                fComplete = false;
                mapSS[vID[j]].Collect(vID[i],mapShare,fComplete);
            }
            fComplete = false;
            ssWitness.Collect(vID[i],mapShare,fComplete);
        }
        std::cout << "\tPublish : " << ((boost::posix_time::microsec_clock::universal_time() - t0).ticks() / count) <<"\n";

        // Reconstruct
        t0 = boost::posix_time::microsec_clock::universal_time();
        for (int i = 0;i < count;i++)
        {
            std::map<uint256,std::pair<uint256,std::size_t> > mapSecret;
            mapSS[vID[i]].Reconstruct(mapSecret);
        }
        std::cout << "\tReconstruct : " << ((boost::posix_time::microsec_clock::universal_time() - t0).ticks() / count) <<"\n";;

        uint256 nRecValue;
        std::map<uint256,std::pair<uint256,std::size_t> > mapSecret;
        ssWitness.Reconstruct(mapSecret);
        for (std::map<uint256,std::pair<uint256,std::size_t> >::iterator it = mapSecret.begin();it != mapSecret.end();++it)
        {
            nRecValue = nRecValue ^ (*it).second.first;
        }
        std::cout << "\tReconstruct value = " << nRecValue.GetHex() << "\n";
        std::cout << "}\n";

        BOOST_CHECK( nRecValue == nInitValue );
    }
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <Eigen/Core>
#include <memory>
#include "ear/common/point_source_panner.hpp"

namespace ear {

  class ExtentPanner {};

  /** @brief Modify an extent parameter given a distance.
   *
   * A right triangle if formed, with the adjacent edge being the distance, and
   * the opposite edge being determined from the extent. The angle formed is
   * then used to determine the new extent.
   *
   * - at distance=0, the extent is always 360
   * - at distance=1, the original extent is used
   * - at distance>1, the extent decreases
   * - in 0 < distance < 1, the extent changes more steeply around 0 for smaller
   *   extents
   */
  double extentMod(double extent, double distance);

  /** @brief Calculate basis vectors that rotate (0, 1, 0)
     onto source_pos. */
  Eigen::Matrix3d calcBasis(Eigen::Vector3d position);

  /** @brief Polar to Cartesian in radians with no distance, in a given basis.*/
  Eigen::Vector3d cartOnBasis(Eigen::Matrix3d basis, double azimuth,
                              double elevation);

  std::pair<double, double> azimuthElevationOnBasis(
      Eigen::Matrix3d basis, Eigen::RowVector3d position);

  /** @brief Weighting function for spread sources.
   *
   * The weighting function is one inside a region approximately determined by a
   * width x height rectangle in azimuth-elevation space, with maximally-sized
   * rounded corners; the shape of the corners is calculated using the vector
   * angle from their centres (always directly above or below the source
   * position) so as to avoid issues at the poles.
   *
   * The two straight edges of the rectangle are always parallel in Cartesian
   * space; this is achieved by following azimuth lines; for tall sources, the
   * whole coordinate system is rotated 90 degrees about the source position to
   * achieve this.
   *
   * Note that for sources where width == height, this degrades to a circular
   * region relative to the source position.
   *
   * To make the two ends meet, the width is adjusted such that a width of 180
   * degrees is mapped to width + height.
   *
   */
  class WeightingFunction {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    /** @brief Ctor
     *
     * @param position Centre of the extent.
     * @param width Width of the extent in degrees from one edge to the other.
     * @param height Height of the extent in degrees from one edge to the other.
     */
    WeightingFunction(Eigen::Vector3d position, double width, double height);

    /** @brief Calculate weight for position */
    double operator()(Eigen::Vector3d position) const;

   private:
    const double _fadeWidth = 10.0;
    double _width;
    double _height;
    double _circleRadius;
    Eigen::Matrix3d _flippedBasis;
    double _circlePos;
    Eigen::Matrix<double, 3, 2> _circlePositions;
  };

  class SpreadingPanner {
   public:
    SpreadingPanner(std::shared_ptr<PointSourcePanner> psp, int nRows);

    /** @brief Panning values for a given weighting function.
     *
     * @param weightFunc function from Cartesian position to weight in range
     * (0, 1)
     *
     * @return panning value for each speaker.
     */
    Eigen::VectorXd panningValuesForWeight(const WeightingFunction& weightFunc);

   private:
    /** @brief Generate points spread evenly on the sphere.
     *
     * Based on
     * http://web.archive.org/web/20150108040043/http://www.math.niu.edu/~rusin/known-math/95/equispace.elect
     *
     * @param nRows number of rows to place on sphere, e.g. 37 for 5 degree
     * spacing
     *
     * @returns cartesian array.
     */
    Eigen::MatrixXd _generatePanningPositionsEven();
    Eigen::MatrixXd _generatePanningPositionsResults();

    std::shared_ptr<PointSourcePanner> _psp;
    int _nRows;
    Eigen::MatrixXd _panningPositions;
    Eigen::MatrixXd _panningPositionsResults;
  };

  class PolarExtentPanner : public ExtentPanner {
   public:
    PolarExtentPanner(std::shared_ptr<PointSourcePanner> psp);

    /** @brief Calculate loudspeaker gains given position and extent parameters.
     *
     * @param position  Cartesian source position
     * @param width block format width parameter
     * @param height block format height parameter
     * @param depth block format depth parameter
     *
     * @returns loudspeaker gains for each channel
     */
    Eigen::VectorXd handle(Eigen::Vector3d position, double width,
                           double height, double depth);

    /** @brief Calculate the speaker panning values for the position, width, and
     * height of a source; this just deals with the positioning and spreading.
     */
    Eigen::VectorXd calcPvSpread(Eigen::Vector3d position, double width,
                                 double height);

   private:
    const int _nRows = 37;  // 5 degrees per row
    const double _fadeWidth = 10.0;  // degrees
    std::shared_ptr<PointSourcePanner> _psp;
    SpreadingPanner _spreadingPanner;
  };

  class CartesianExtentPanner : public ExtentPanner {};

}  // namespace ear


/*
 [auto_generated]
 boost/numeric/odeint/stepper/implicit_euler.hpp

 [begin_description]
 Impementation of the implicit Euler method. Works with ublas::vector as state type.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef OMPLEXT_BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED
#define OMPLEXT_BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED

#include <utility>

#include <omplext_odeint/boost/numeric/odeint/util/bind.hpp>
#include <omplext_odeint/boost/numeric/odeint/util/unwrap_reference.hpp>
#include <omplext_odeint/boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <omplext_odeint/boost/numeric/odeint/util/ublas_wrapper.hpp>
#include <omplext_odeint/boost/numeric/odeint/util/is_resizeable.hpp>
#include <omplext_odeint/boost/numeric/odeint/util/resizer.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace boost {
namespace numeric {
namespace omplext_odeint {

template< class ValueType , class Resizer = initially_resizer >
class implicit_euler
{

public:

    typedef ValueType value_type;
    typedef value_type time_type;
    typedef boost::numeric::ublas::vector< value_type > state_type;
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_type deriv_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef boost::numeric::ublas::matrix< value_type > matrix_type;
    typedef state_wrapper< matrix_type > wrapped_matrix_type;
    typedef boost::numeric::ublas::permutation_matrix< size_t > pmatrix_type;
    typedef state_wrapper< pmatrix_type > wrapped_pmatrix_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;
    typedef implicit_euler< ValueType , Resizer > stepper_type;

    implicit_euler( value_type epsilon = 1E-6 )
    : m_epsilon( epsilon )
    { }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt )
    {
        typedef typename omplext_odeint::unwrap_reference< System >::type system_type;
        typedef typename omplext_odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
        typedef typename omplext_odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
        system_type &sys = system;
        deriv_func_type &deriv_func = sys.first;
        jacobi_func_type &jacobi_func = sys.second;

        m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );

        for( size_t i=0 ; i<x.size() ; ++i )
            m_pm.m_v[i] = i;

        t += dt;

        // apply first Newton step
        deriv_func( x , m_dxdt.m_v , t );

        m_b.m_v = dt * m_dxdt.m_v;

        jacobi_func( x , m_jacobi.m_v  , t );
        m_jacobi.m_v *= dt;
        m_jacobi.m_v -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );

        solve( m_b.m_v , m_jacobi.m_v );

        m_x.m_v = x - m_b.m_v;

        // iterate Newton until some precision is reached
        // ToDo: maybe we should apply only one Newton step -> linear implicit one-step scheme
        while( boost::numeric::ublas::norm_2( m_b.m_v ) > m_epsilon )
        {
            deriv_func( m_x.m_v , m_dxdt.m_v , t );
            m_b.m_v = x - m_x.m_v + dt*m_dxdt.m_v;

            // simplified version, only the first Jacobian is used
            //            jacobi( m_x , m_jacobi , t );
            //            m_jacobi *= dt;
            //            m_jacobi -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );

            solve( m_b.m_v , m_jacobi.m_v );

            m_x.m_v -= m_b.m_v;
        }
        x = m_x.m_v;
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_b , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable<matrix_type>::type() );
        resized |= adjust_size_by_resizeability( m_pm , x , typename is_resizeable<pmatrix_type>::type() );
        return resized;
    }

    void solve( state_type &x , matrix_type &m )
    {
        int res = boost::numeric::ublas::lu_factorize( m , m_pm.m_v );
        if( res != 0 ) exit(0);
        boost::numeric::ublas::lu_substitute( m , m_pm.m_v , x );
    }

private:

    value_type m_epsilon;
    resizer_type m_resizer;
    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_x;
    wrapped_deriv_type m_b;
    wrapped_matrix_type m_jacobi;
    wrapped_pmatrix_type m_pm;

};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED


//
//  cal_ezz.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/7/18.
//
//

#ifndef cal_ezz_hpp
#define cal_ezz_hpp

#include <stdio.h>
#include <Eigen/Eigen>

using namespace Eigen;

void cal_ezz(MatrixXd coord ,double E, double nu,std::vector<double> &ezz_out, std::vector<double> &exx_out,std::vector<double> &eyy_out,  int n_el, MatrixXi &index_store, double q, VectorXd &u_n);
#endif /* cal_ezz_hpp */


#pragma once

#include <memory>
#include <vector>
#include <Eigen/Core>

/* Custom exceptions for the k-d tree*/

struct VaryingStateSizeException : public std::exception
{
    const char* what() const throw()
    {
        return "Cannot build a tree with varying state sizes.";
    }
};

struct EmptyTreeException : public std::exception
{
    const char* what() const throw()
    {
        return "Cannot perform this operation on an empty tree.";
    }
};

/* Tree Node Class */

struct kdNode {
    Eigen::VectorXd state;

    std::shared_ptr<kdNode> left;
    std::shared_ptr<kdNode> right;
};

/* Tree Class */

class kdTree {
public:
    kdTree() {n_states = 0;}
    kdTree(const std::vector<Eigen::VectorXd>& states);
    kdTree(const Eigen::MatrixXd& states);

    int state_size;
    int n_states;

    void append_state(const Eigen::VectorXd& state);

    Eigen::VectorXd nearest_neighbor(const Eigen::VectorXd& search_state) const;

    Eigen::MatrixXd k_nearest_neighbors(const Eigen::VectorXd& search_state, int k) const;

    Eigen::MatrixXd at_depth(int depth) const;

    int count_states() const;

private:
    std::shared_ptr<kdNode> root;
};


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("gen_linspace_1")
  {
  vec a = linspace(1,5,5);

  REQUIRE(a(0) == Approx(1.0));
  REQUIRE(a(1) == Approx(2.0));
  REQUIRE(a(2) == Approx(3.0));
  REQUIRE(a(3) == Approx(4.0));
  REQUIRE(a(4) == Approx(5.0));

  vec b = linspace<vec>(1,5,6);

  REQUIRE(b(0) == Approx(1.0));
  REQUIRE(b(1) == Approx(1.8));
  REQUIRE(b(2) == Approx(2.6));
  REQUIRE(b(3) == Approx(3.4));
  REQUIRE(b(4) == Approx(4.2));
  REQUIRE(b(5) == Approx(5.0));

  rowvec c = linspace<rowvec>(1,5,6);

  REQUIRE(c(0) == Approx(1.0));
  REQUIRE(c(1) == Approx(1.8));
  REQUIRE(c(2) == Approx(2.6));
  REQUIRE(c(3) == Approx(3.4));
  REQUIRE(c(4) == Approx(4.2));
  REQUIRE(c(5) == Approx(5.0));

  mat X = linspace<mat>(1,5,6);

  REQUIRE(X.n_rows == 6);
  REQUIRE(X.n_cols == 1);
  }


// sphere_fit.cpp
//
// Example from Foundations of Geometric Algebra Computing
// by Dietmar Hildenbrand
//
// Page 68.

#include <iostream>
#include <string>
#include <boost/numpy.hpp>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <hep/ga.hpp>

#include "game/types.h"
#include "game/ceres_python_utils.h"

namespace bp = boost::python;
namespace np = boost::numpy;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;

namespace game {

struct SphereFit {
  SphereFit(const SphereFit &sphere_fit) {}
  SphereFit() {}

  void SetSolverOptions(const bp::dict &solver_options) {
    bp::extract<std::string> linear_solver_type(
        solver_options["linear_solver_type"]);
    if (linear_solver_type.check()) {
      ceres::StringToLinearSolverType(linear_solver_type(),
                                      &options_.linear_solver_type);
    }

    bp::extract<int> max_num_iterations(solver_options["max_num_iterations"]);
    if (max_num_iterations.check()) {
      options_.max_num_iterations = max_num_iterations();
    }

    bp::extract<int> num_threads(solver_options["num_threads"]);
    if (num_threads.check()) {
      options_.num_threads = num_threads();
    }

    bp::extract<int> num_linear_solver_threads(
        solver_options["num_linear_solver_threads"]);
    if (num_linear_solver_threads.check()) {
      options_.num_linear_solver_threads = num_linear_solver_threads();
    }

    bp::extract<double> parameter_tolerance(
        solver_options["parameter_tolerance"]);
    if (parameter_tolerance.check()) {
      options_.parameter_tolerance = parameter_tolerance();
    }

    bp::extract<double> function_tolerance(
        solver_options["function_tolerance"]);
    if (function_tolerance.check()) {
      options_.function_tolerance = function_tolerance();
    }

    bp::extract<std::string> trust_region_strategy_type(
        solver_options["trust_region_strategy_type"]);
    if (trust_region_strategy_type.check()) {
      ceres::StringToTrustRegionStrategyType(
          trust_region_strategy_type(), &options_.trust_region_strategy_type);
    }

    bp::extract<bool> minimizer_progress_to_stdout(
        solver_options["minimizer_progress_to_stdout"]);
    if (minimizer_progress_to_stdout.check()) {
      options_.minimizer_progress_to_stdout = minimizer_progress_to_stdout();
    }

    bp::extract<std::string> minimizer_type(solver_options["minimizer_type"]);
    if (minimizer_type.check()) {
      ceres::StringToMinimizerType(minimizer_type(), &options_.minimizer_type);
    }

    bp::extract<bp::list> trust_region_minimizer_iterations_to_dump(
        solver_options["trust_region_minimizer_iterations_to_dump"]);
    if (trust_region_minimizer_iterations_to_dump.check()) {
      std::vector<int> iterations_to_dump{};
      bp::list list = trust_region_minimizer_iterations_to_dump();
      for (int i = 0; i < bp::len(list); ++i) {
        iterations_to_dump.push_back(bp::extract<int>(list[i])());
      }
      options_.trust_region_minimizer_iterations_to_dump = iterations_to_dump;
    }

    bp::extract<std::string> trust_region_problem_dump_directory(
        solver_options["trust_region_problem_dump_directory"]);
    if (trust_region_problem_dump_directory.check()) {
      options_.trust_region_problem_dump_directory =
          trust_region_problem_dump_directory();
    }
  }

  template <typename T, typename MultivectorT>
  static auto Inverse(const MultivectorT &multivector) -> MultivectorT {
    return cga::Scalar<T>{static_cast<T>(1.0) /
                          hep::eval((multivector * ~multivector))[0]} *
           ~multivector;
  }

  struct CostFunctor {
    CostFunctor(const double *point) : point_(point) {}

    template <typename T>
    bool operator()(const T *const s /* sphere: 5 parameters */,
                    T *residual /* 1 parameter */) const {
      // Conformal split
      cga::Infty<T> ni{static_cast<T>(-1.0), static_cast<T>(1.0)};
      cga::Orig<T> no{static_cast<T>(0.5), static_cast<T>(0.5)};

      // Euclidean basis
      cga::E1<T> e1{static_cast<T>(1.0)};
      cga::E2<T> e2{static_cast<T>(1.0)};
      cga::E3<T> e3{static_cast<T>(1.0)};

      cga::Scalar<T> half{static_cast<T>(0.5)};

      // Create Euclidean point (vector)
      cga::EuclideanPoint<T> euc_point{static_cast<T>(point_[0]),
                                       static_cast<T>(point_[1]),
                                       static_cast<T>(point_[2])};

      // Create conformal point
      cga::Point<T> point =
          hep::grade<1>(euc_point + half * euc_point * euc_point * ni + no);

      // Create conformal sphere
      //      cga::Sphere<T> sphere = hep::eval(
      //          static_cast<T>(s[0] / s[3]) * e1 + static_cast<T>(s[1] / s[3])
      //          * e2 +
      //          static_cast<T>(s[2] / s[3]) * e3 + static_cast<T>(s[4] / s[3])
      //          * ni +
      //          static_cast<T>(s[3] / s[3]) * no);

      cga::Sphere<T> sphere =
          hep::eval(static_cast<T>(s[0]) * e1 + static_cast<T>(s[1]) * e2 +
                    static_cast<T>(s[2]) * e3 + static_cast<T>(s[4]) * ni +
                    static_cast<T>(s[3]) * no);

      // Evaluate distance
      //      auto distance = hep::eval(hep::inner_prod(point, sphere) *
      //                                hep::inner_prod(point,
      //                                Inverse<T>(sphere)));

      auto distance = hep::eval(hep::inner_prod(point, sphere));
      auto rho_squared = hep::eval(hep::inner_prod(sphere, sphere));

      residual[0] = distance[0] / sqrt(rho_squared[0]);
      //      residual[0] = distance[0];

      return true;
    }

   private:
    const double *point_;
  };

  static ceres::CostFunction *Create(const double *point) {
    return (new ceres::AutoDiffCostFunction<SphereFit::CostFunctor, 1, 5>(
        new SphereFit::CostFunctor(point)));
  }

  np::ndarray Run(np::ndarray sphere, np::ndarray points) {
    if (!(points.get_flags() & np::ndarray::C_CONTIGUOUS)) {
      throw "input array a must be contiguous";
    }

    auto rows_points = points.shape(0);
    auto cols_points = points.shape(1);
    auto rows_sphere = sphere.shape(0);
    auto cols_sphere = sphere.shape(1);

    if (!((rows_sphere == 5) && (cols_sphere == 1))) {
      throw "parameter array must have shape (5,1)";
    }

    double *sphere_data = reinterpret_cast<double *>(sphere.get_data());
    double *points_data = reinterpret_cast<double *>(points.get_data());

    for (int i = 0; i < rows_points; ++i) {
      ceres::CostFunction *cost_function =
          SphereFit::Create(&points_data[cols_points * i]);
      problem_.AddResidualBlock(cost_function, NULL, sphere_data);
    }

    Solve(options_, &problem_, &summary_);

    return sphere;
  }

  auto Summary() -> bp::dict { return game::SummaryToDict(summary_); }

  Problem problem_;
  Solver::Options options_;
  Solver::Summary summary_;
};
}

BOOST_PYTHON_MODULE(libsphere_fit) {
  np::initialize();

  bp::class_<game::SphereFit>("SphereFit")
      .def("run", &game::SphereFit::Run)
      .def("summary", &game::SphereFit::Summary)
      .def("set_solver_options", &game::SphereFit::SetSolverOptions);
}


#include <ancse/cfl_condition.hpp>

#include <Eigen/Dense>

// define FVM CFL condition functions defined in cfl_condition.hpp
FVMCFLCondition::FVMCFLCondition(const Grid& grid,
                                 const std::shared_ptr<Model>& model,
                                 double cfl_number)
    : grid(grid), model(model), cfl_number(cfl_number) {}

double FVMCFLCondition::operator() (const Eigen::MatrixXd& u) const
{
    const int n_cells= grid.n_cells;
    const int n_ghost= grid.n_ghost;

    const double dx= grid.dx;

    double a_max= 0.0;

    for (int i= n_ghost; i < n_cells - n_ghost; ++i)
        a_max= std::max(a_max, model->max_eigenvalue(u.col(i)));

    return cfl_number * dx / a_max;
}

// define DG CFL condition functions defined in cfl_condition.hpp
DGCFLCondition::DGCFLCondition(const Grid& grid,
                               const std::shared_ptr<Model>& model,
                               const DGHandler& dg_handler,
                               double cfl_number)
    : grid(grid), model(model), dg_handler(dg_handler), cfl_number(cfl_number) {}

double DGCFLCondition::operator() (const Eigen::MatrixXd& u) const
{
    const int n_cells= grid.n_cells;
    const int n_ghost= grid.n_ghost;

    const double dx= grid.dx;

    double a_max= 0.0;

    Eigen::MatrixXd u0= dg_handler.build_cell_avg(u);

    for (int i= n_ghost; i < n_cells - n_ghost; ++i)
        a_max= std::max(a_max, model->max_eigenvalue(u0.col(i)));

    return cfl_number * dx / a_max;
}

/// make CFL condition for FVM
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid& grid,
                   const std::shared_ptr<Model>& model,
                   double cfl_number)
{
    // implement this 'factory' for your CFL condition.
    return std::make_shared<FVMCFLCondition>(grid, model, cfl_number);
}

/// make CFL condition for DG
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid& grid,
                   const std::shared_ptr<Model>& model,
                   const DGHandler& dg_handler,
                   double cfl_number)
{
    // implement this 'factory' for your CFL condition.
    return std::make_shared<DGCFLCondition>(grid, model, dg_handler, cfl_number);
}


// $Id: interpolation_test.cpp 1302 2015-06-03 09:24:11Z veralh $

/// \file Unit tests for the Spline function in geometry

#include <nrlib/geometry/interpolation.hpp>

#include <boost/test/unit_test.hpp>
#include <math.h>
#include <algorithm>

using namespace NRLib;

BOOST_AUTO_TEST_CASE( Linear1D )
{
  size_t n = 10;
  double x_start = -1;
  double x_end   = 3.25;
  double dx = (x_end-x_start)/(n-1);

  double const_value = -5.3;
  std::vector<double> x_in(n, -1.0);
  std::vector<double> y_in_const (n, const_value);
  std::vector<double> y_in_linear(n, -1.0);

  for (size_t i = 0; i < x_in.size(); ++i){
    x_in[i] = x_start + i*dx;
    double x = x_in[i];

    y_in_linear[i] = 10.1 - 0.4*x;
  }

  n = 37;
  dx = (x_end - x_start)/(n-1);
  std::vector<double> x_out(n, -1.0);
  for (size_t i = 0; i < x_out.size(); ++i){
    x_out[i] = x_start + i*dx;
  }

  std::vector<double> y_out_const  = Interpolation::Interpolate1D(x_in, y_in_const,  x_out, "linear");
  std::vector<double> y_out_linear = Interpolation::Interpolate1D(x_in, y_in_linear, x_out, "linear");

  for(size_t i = 0; i < x_out.size(); ++i )
  {
    double x = x_out[i];
    BOOST_CHECK_CLOSE(y_out_const [i], const_value,  0.01);
    BOOST_CHECK_CLOSE(y_out_linear[i], 10.1 - 0.4*x, 0.01);
  }
}

BOOST_AUTO_TEST_CASE( Linear1DWithExtrapolation )
{
  size_t n = 10;
  double x_start = -1;
  double x_end   = 3.25;
  double dx = (x_end-x_start)/(n-1);

  double const_value = -5.3;
  std::vector<double> x_in(n, -1.0);
  std::vector<double> y_in_const (n, const_value);
  std::vector<double> y_in_linear(n, -1.0);

  for (size_t i = 0; i < x_in.size(); ++i){
    x_in[i] = x_start + i*dx;
    double x = x_in[i];

    y_in_linear[i] = 10.1 - 0.4*x;
  }

  n = 37;
  x_start = -10;
  x_end   = 5.5;
  dx = (x_end - x_start)/(n-1);
  std::vector<double> x_out(n, -1.0);
  for (size_t i = 0; i < x_out.size(); ++i){
    x_out[i] = x_start + i*dx;
  }

  std::vector<double> y_out_const  = Interpolation::Interpolate1D(x_in, y_in_const,  x_out, "linear");
  std::vector<double> y_out_linear = Interpolation::Interpolate1D(x_in, y_in_linear, x_out, "linear");

  for(size_t i = 0; i < x_out.size(); ++i )
  {
    double x = x_out[i];
    BOOST_CHECK_CLOSE(y_out_const [i], const_value,  0.01);
    BOOST_CHECK_CLOSE(y_out_linear[i], 10.1 - 0.4*x, 0.01);
  }
}

BOOST_AUTO_TEST_CASE( Spline1D )
{
  size_t n = 10;
  double x_start = 0;
  double x_end   = 2.25;
  double dx = (x_end-x_start)/(n-1);

  double const_value = 2.4;
  std::vector<double> x_in(n, -1.0);
  std::vector<double> y_in_const (n, const_value);
  std::vector<double> y_in_linear(n, -1.0);
  std::vector<double> y_in_square(n, -1.0);
  std::vector<double> y_in_cubic (n, -1.0);
  std::vector<double> y_in_sin   (n, -1.0);
  std::vector<double> y_in_sqrt  (n, -1.0);

  for (size_t i = 0; i < x_in.size(); ++i){
    x_in[i] = x_start + i*dx;
    double x = x_in[i];

    y_in_linear[i] = -4.4 + 2.3*x;
    y_in_square[i] = x*x;
    y_in_cubic [i] = -3 + x + 5.5*x*x - 4.4*x*x*x;
    y_in_sin   [i] = sin(x);
    y_in_sqrt  [i] = sqrt(x);
  }

  n = 37;
  dx = (x_end - x_start)/(n-1);
  std::vector<double> x_out(n, -1.0);
  for (size_t i = 0; i < x_out.size(); ++i){
    x_out[i] = x_start + i*dx;
  }

  std::vector<double> y_out_const  = Interpolation::Interpolate1D(x_in, y_in_const,  x_out, "spline");
  std::vector<double> y_out_linear = Interpolation::Interpolate1D(x_in, y_in_linear, x_out, "spline");
  std::vector<double> y_out_square = Interpolation::Interpolate1D(x_in, y_in_square, x_out, "spline");
  std::vector<double> y_out_cubic  = Interpolation::Interpolate1D(x_in, y_in_cubic,  x_out, "spline");
  std::vector<double> y_out_sin    = Interpolation::Interpolate1D(x_in, y_in_sin,    x_out, "spline");
  std::vector<double> y_out_sqrt   = Interpolation::Interpolate1D(x_in, y_in_sqrt,   x_out, "spline");

  for(size_t i = 0; i < x_out.size(); ++i )
  {
    double x = x_out[i];
    if(std::find(x_in.begin(), x_in.end(), x) != x_in.end())
    {
      // Exact at knots
      BOOST_CHECK_CLOSE(y_out_const [i], const_value,  1.0e-7);
      BOOST_CHECK_CLOSE(y_out_linear[i], -4.4 + 2.3*x, 1.0e-7);
      BOOST_CHECK_CLOSE(y_out_square[i], x*x,          1.0e-7);
      BOOST_CHECK_CLOSE(y_out_cubic [i], -3 + x + 5.5*x*x - 4.4*x*x*x, 1.0e-7);
      BOOST_CHECK_CLOSE(y_out_sin   [i], sin(x),       1.0e-7);
      BOOST_CHECK_CLOSE(y_out_sqrt  [i], sqrt(x),      1.0e-7);
    }
    else if(x > x_in[1])
    {
      // Not comparing in first interval
      BOOST_CHECK_CLOSE(y_out_const [i], const_value,  0.1);
      BOOST_CHECK_CLOSE(y_out_linear[i], -4.4 + 2.3*x, 0.1);
      BOOST_CHECK_CLOSE(y_out_square[i], x*x,          2.0);
      BOOST_CHECK_CLOSE(y_out_cubic [i], -3 + x + 5.5*x*x - 4.4*x*x*x, 1.0);
      BOOST_CHECK_CLOSE(y_out_sin   [i], sin(x),       2.0);
      BOOST_CHECK_CLOSE(y_out_sqrt  [i], sqrt(x),      2.8);
    }
  }
}

BOOST_AUTO_TEST_CASE( Spline1DExtrapolation)
{

  size_t n = 11;
  double x_start = -1;
  double x_end   = 4;
  double dx = (x_end-x_start)/(n-1);

  std::vector<double> x_in(n, -1.0);
  std::vector<double> y_in_cubic (n, -1.0);
  std::vector<double> y_in_sin   (n, -1.0);

  for (size_t i = 0; i < x_in.size(); ++i){
    x_in[i] = x_start + i*dx;
    double x = x_in[i];

    y_in_cubic[i] = -3 + x + 5.5*x*x - 4.4*x*x*x;;
    y_in_sin[i]   = sin(x);
  }

  n = 61;
  x_start = -6;
  x_end   = 9;
  dx = (x_end - x_start)/(n-1);
  std::vector<double> x_out(n, -1.0);
  for (size_t i = 0; i < x_out.size(); ++i){
    x_out[i] = x_start + i*dx;
  }

  std::vector<double> y_out_cubic_extrap = Interpolation::Interpolate1D(x_in, y_in_cubic,  x_out, "spline", 0.0);
  std::vector<double> y_out_sin_extrap   = Interpolation::Interpolate1D(x_in, y_in_sin,    x_out, "spline", 0.0);

  for(size_t i = 0; i < x_out.size(); ++i ){
    double x = x_out[i];

    // Check for equality to pre-set extrapapolation value outside defined intervals
    if(x < x_in[0] || x > x_in[x_in.size()-1])
    {
      BOOST_CHECK_EQUAL(y_out_cubic_extrap [i], 0.0);
      BOOST_CHECK_EQUAL(y_out_sin_extrap   [i], 0.0);
    }

    // Check for equality at the knots
    else if(std::find(x_in.begin(), x_in.end(), x) != x_in.end())
    {
      // Exact at knots
      BOOST_CHECK_CLOSE(y_out_cubic_extrap [i], -3 + x + 5.5*x*x - 4.4*x*x*x, 1.0e-7);
      BOOST_CHECK_CLOSE(y_out_sin_extrap   [i], sin(x),                       1.0e-7);

    }

    // Check for close value inside defined intervals
    else if(x > x_in[0] && x < x_in[2]) {
      // In first two interval we allow larger mismatch
      BOOST_CHECK_CLOSE(y_out_cubic_extrap [i], -3 + x + 5.5*x*x - 4.4*x*x*x, 36.0);
      BOOST_CHECK_CLOSE(y_out_sin_extrap   [i], sin(x),                       20.0);
    }

    else {
      BOOST_CHECK_CLOSE(y_out_cubic_extrap [i], -3 + x + 5.5*x*x - 4.4*x*x*x, 1.6);
      BOOST_CHECK_CLOSE(y_out_sin_extrap   [i], sin(x),                       2.2);
    }

  }
}