/*
    Copyright (c) 2013, Taiga Nomi
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
    * 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.
    * Neither the name of the <organization> nor the
    names of its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

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

using namespace tiny_cnn;
using namespace tiny_cnn::activation;

TEST(convolutional, fprop) {
    typedef network<mse, gradient_descent_levenberg_marquardt> CNN;
    CNN nn;

    convolutional_layer<sigmoid> l(5, 5, 3, 1, 2);

    vec_t in(25);

    ASSERT_EQ(l.weight().size(), 18);

    std::fill(l.bias().begin(), l.bias().end(), 0.0);
    std::fill(l.weight().begin(), l.weight().end(), 0.0);

    uniform_rand(in.begin(), in.end(), -1.0, 1.0);

    {
        const vec_t& out = l.forward_propagation(in, 0);

        for (auto o: out)
            EXPECT_DOUBLE_EQ(o, (tiny_cnn::float_t)0.5);

    }

    l.weight()[0] = 0.3;  l.weight()[1] = 0.1; l.weight()[2] = 0.2;
    l.weight()[3] = 0.0;  l.weight()[4] =-0.1; l.weight()[5] =-0.1;
    l.weight()[6] = 0.05; l.weight()[7] =-0.2; l.weight()[8] = 0.05;

    l.weight()[9]  = 0.0; l.weight()[10] =-0.1; l.weight()[11] = 0.1;
    l.weight()[12] = 0.1; l.weight()[13] =-0.2; l.weight()[14] = 0.3;
    l.weight()[15] = 0.2; l.weight()[16] =-0.3; l.weight()[17] = 0.2;

    in[0] = 3;  in[1] = 2;  in[2] = 1;  in[3] = 5; in[4] = 2;
    in[5] = 3;  in[6] = 0;  in[7] = 2;  in[8] = 0; in[9] = 1;
    in[10] = 0; in[11] = 6; in[12] = 1; in[13] = 1; in[14] = 10;
    in[15] = 3; in[16] =-1; in[17] = 2; in[18] = 9; in[19] = 0;
    in[20] = 1; in[21] = 2; in[22] = 1; in[23] = 5; in[24] = 5;

    {
        const vec_t& out = l.forward_propagation(in, 0);

        EXPECT_NEAR(0.4875026, out[0], 1E-5);
        EXPECT_NEAR(0.8388910, out[1], 1E-5);
        EXPECT_NEAR(0.8099984, out[2], 1E-5);
        EXPECT_NEAR(0.7407749, out[3], 1E-5);
        EXPECT_NEAR(0.5000000, out[4], 1E-5);
        EXPECT_NEAR(0.1192029, out[5], 1E-5);
        EXPECT_NEAR(0.5986877, out[6], 1E-5);
        EXPECT_NEAR(0.7595109, out[7], 1E-5);
        EXPECT_NEAR(0.6899745, out[8], 1E-5);
    }

}

TEST(convolutional, bprop) {
    network<cross_entropy, gradient_descent_levenberg_marquardt> nn;

    nn << convolutional_layer<sigmoid>(5, 5, 3, 1, 1);

    vec_t a(25, 0.0), t(9, 0.0);
    std::vector<vec_t> data, train;

    for (int y = 0; y < 5; y++) {
        a[5*y+3] = 1.0;
    }

    for (int y = 0; y < 3; y++) {
        t[3*y+0] = 0.0;
        t[3*y+1] = 0.5;
        t[3*y+2] = 1.0;
    }

    for (int i = 0; i < 100; i++) {
        data.push_back(a);
        train.push_back(t);
    }

    nn.train(data, train);

    vec_t predicted = nn.predict(a);
}

TEST(convolutional, gradient_check) { // tanh - mse
    network<mse, gradient_descent_levenberg_marquardt> nn;
    nn << convolutional_layer<tan_h>(5, 5, 3, 1, 1);

    vec_t a(25, 0.0);
    label_t t = 3;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_ALL));
}

TEST(convolutional, gradient_check2) { // sigmoid - mse
    network<mse, gradient_descent_levenberg_marquardt> nn;
    nn << convolutional_layer<sigmoid>(5, 5, 3, 1, 1);

    vec_t a(25, 0.0);
    label_t t = 3;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_ALL));
}

TEST(convolutional, gradient_check3) { // rectified - mse
    network<mse, gradient_descent_levenberg_marquardt> nn;

    nn << convolutional_layer<rectified_linear>(5, 5, 3, 1, 1);

    vec_t a(25, 0.0);
    label_t t = 3;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_ALL));
}

TEST(convolutional, gradient_check4) { // identity - mse
    network<mse, gradient_descent_levenberg_marquardt> nn;

    nn << convolutional_layer<identity>(5, 5, 3, 1, 1);

    vec_t a(25, 0.0);
    label_t t = 3;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_ALL));
}

TEST(convolutional, gradient_check5) { // sigmoid - cross-entropy
    network<cross_entropy, gradient_descent_levenberg_marquardt> nn;

    nn << convolutional_layer<sigmoid>(5, 5, 3, 1, 1);

    vec_t a(25, 0.0);
    label_t t = 3;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_ALL));
}

TEST(convolutional, serialize) {
    convolutional_layer<tan_h> layer1(14, 14, 5, 1, 2);
    convolutional_layer<tan_h> layer2(14, 14, 5, 1, 2);

    vec_t v(14*14);

    uniform_rand(v.begin(), v.end(), -1.0, 1.0);
    layer1.init_weight();

    std::ostringstream os;
    layer1.save(os);

    std::istringstream is(os.str());
    layer2.load(is);

    const vec_t& out1 = layer1.forward_propagation(v, 0);
    const vec_t& out2 = layer2.forward_propagation(v, 0);

    for (size_t i = 0; i < out1.size(); i++)
        EXPECT_NEAR(out1[i], out2[i], 1e-4);
}

TEST(convolutional, serialize2) {
#define O true
#define X false
    static const bool connection[] = {
        O, X, X, X, O, O,
        O, O, X, X, X, O,
        O, O, O, X, X, X
    };
#undef O
#undef X
    convolutional_layer<tan_h> layer1(14, 14, 5, 3, 6, connection_table(connection, 3, 6));
    convolutional_layer<tan_h> layer2(14, 14, 5, 3, 6, connection_table(connection, 3, 6));

    vec_t v(14*14*3);

    uniform_rand(v.begin(), v.end(), -1.0, 1.0);
    layer1.init_weight();

    std::ostringstream os;
    layer1.save(os);

    std::istringstream is(os.str());
    layer2.load(is);

    const vec_t& out1 = layer1.forward_propagation(v, 0);
    const vec_t& out2 = layer2.forward_propagation(v, 0);

    for (size_t i = 0; i < out1.size(); i++)
        EXPECT_NEAR(out1[i], out2[i], 1e-4);
}

TEST(fully_connected, bprop) {
    network<cross_entropy, gradient_descent_levenberg_marquardt> nn;

    nn << fully_connected_layer<sigmoid>(3, 2);

    vec_t a(3), t(2), a2(3), t2(2);

    a[0] = 3.0; a[1] = 0.0; a[2] = -1.0;
    t[0] = 0.3; t[1] = 0.7;

    a2[0] = 0.2; a2[1] = 0.5; a2[2] = 4.0;
    t2[0] = 0.5; t2[1] = 0.1;

    std::vector<vec_t> data, train;

    for (int i = 0; i < 100; i++) {
        data.push_back(a);
        data.push_back(a2);
        train.push_back(t);
        train.push_back(t2);
    }
    nn.optimizer().alpha = 0.1;
    nn.train(data, train, 1, 10);

    vec_t predicted = nn.predict(a);

    EXPECT_NEAR(predicted[0], t[0], 1E-5);
    EXPECT_NEAR(predicted[1], t[1], 1E-5);

    predicted = nn.predict(a2);

    EXPECT_NEAR(predicted[0], t2[0], 1E-5);
    EXPECT_NEAR(predicted[1], t2[1], 1E-5);
}

TEST(fully_connected, serialize) {
    fully_connected_layer<tan_h> layer1(10, 10);
    fully_connected_layer<tan_h> layer2(10, 10);

    vec_t v(10);

    uniform_rand(v.begin(), v.end(), -1.0, 1.0);
    layer1.init_weight();

    std::ostringstream os;
    layer1.save(os);

    std::istringstream is(os.str());
    layer2.load(is);

    const vec_t& out1 = layer1.forward_propagation(v, 0);
    const vec_t& out2 = layer2.forward_propagation(v, 0);

    for (size_t i = 0; i < out1.size(); i++)
        EXPECT_NEAR(out1[i], out2[i], 1e-4);
}

TEST(fully_connected, bprop2) {
    network<mse, gradient_descent> nn;

    nn << fully_connected_layer<tan_h>(4, 6)
       << fully_connected_layer<tan_h>(6, 3);

    vec_t a(4, 0.0), t(3, 0.0), a2(4, 0.0), t2(3, 0.0);

    a[0] = 3.0; a[1] = 1.0; a[2] = -1.0; a[3] = 4.0;
    t[0] = 0.3; t[1] = 0.7; t[2] = 0.3;

    a2[0] = 1.0; a2[1] = 0.0; a2[2] = 4.0; a2[3] = 2.0;
    t2[0] = 0.6; t2[1] = 0.0; t2[2] = 0.1;

    std::vector<vec_t> data, train;

    for (int i = 0; i < 100; i++) {
        data.push_back(a);
        data.push_back(a2);
        train.push_back(t);
        train.push_back(t2);
    }
    nn.optimizer().alpha = 0.01;
    nn.train(data, train, 1, 10);

    vec_t predicted = nn.predict(a);

    EXPECT_NEAR(predicted[0], t[0], 1E-4);
    EXPECT_NEAR(predicted[1], t[1], 1E-4);

    predicted = nn.predict(a2);

    EXPECT_NEAR(predicted[0], t2[0], 1E-4);
    EXPECT_NEAR(predicted[1], t2[1], 1E-4);
}

TEST(multi_layer, gradient_check) { // sigmoid - cross-entropy
    typedef cross_entropy loss_func;
    typedef sigmoid activation;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    network nn;
    nn << fully_connected_layer<activation>(10, 14*14*3)
       << convolutional_layer<activation>(14, 14, 5, 3, 6)
       << average_pooling_layer<activation>(10, 10, 6, 2)
       << fully_connected_layer<activation>(5*5*6, 3);

    vec_t a(10, 0.0);
    label_t t = 2;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_RANDOM));
}

TEST(multi_layer, gradient_check2) { // tan_h - mse
    typedef mse loss_func;
    typedef tan_h activation;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    network nn;
    nn << fully_connected_layer<activation>(10, 14 * 14 * 3)
        << convolutional_layer<activation>(14, 14, 5, 3, 6)
        << average_pooling_layer<activation>(10, 10, 6, 2)
        << fully_connected_layer<activation>(5 * 5 * 6, 3);

    vec_t a(10, 0.0);
    label_t t = 2;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_RANDOM));
}

TEST(multi_layer, gradient_check3) { // mixture - mse
    typedef mse loss_func;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    network nn;
    nn << fully_connected_layer<tan_h>(10, 14 * 14 * 3)
        << convolutional_layer<sigmoid>(14, 14, 5, 3, 6)
        << average_pooling_layer<rectified_linear>(10, 10, 6, 2)
        << fully_connected_layer<identity>(5 * 5 * 6, 3);

    vec_t a(10, 0.0);
    label_t t = 2;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_RANDOM));
}

TEST(multi_layer4, gradient_check) { // sigmoid - cross-entropy
    typedef cross_entropy loss_func;
    typedef sigmoid activation;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    network nn;
    nn << fully_connected_layer<activation>(10, 14 * 14 * 3)
        << convolutional_layer<activation>(14, 14, 5, 3, 6)
        << average_pooling_layer<activation>(10, 10, 6, 2)
        << fully_connected_layer<activation>(5 * 5 * 6, 3);

    vec_t a(10, 0.0);
    label_t t = 2;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-4, GRAD_CHECK_RANDOM));
}

TEST(multi_layer5, gradient_check) { // softmax - cross-entropy
    typedef cross_entropy loss_func;
    typedef softmax activation;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    network nn;
    nn << fully_connected_layer<activation>(10, 14 * 14 * 3)
        << convolutional_layer<activation>(14, 14, 5, 3, 6)
        << average_pooling_layer<activation>(10, 10, 6, 2)
        << fully_connected_layer<activation>(5 * 5 * 6, 3);

    vec_t a(10, 0.0);
    label_t t = 2;

    uniform_rand(a.begin(), a.end(), -1, 1);
    nn.init_weight();
    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 5e-3, GRAD_CHECK_RANDOM));
}

TEST(max_pool, gradient_check) { // sigmoid - cross-entropy
    typedef cross_entropy loss_func;
    typedef sigmoid activation;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    network nn;
    nn << fully_connected_layer<activation>(3, 8)
       << max_pooling_layer<activation>(4, 2, 1, 2); // 4x2 => 2x1

    vec_t a(3, 0.0);
    for (int i = 0; i < 3; i++) a[i] = i;
    label_t t = 0;

    nn.init_weight();
    for (int i = 0; i < 24; i++) nn[0]->weight()[i] = i;

    EXPECT_TRUE(nn.gradient_check(&a, &t, 1, 1e-5, GRAD_CHECK_ALL));
}

template <typename T>
void serialization_test(const T& src, T& dst)
{
    EXPECT_FALSE(src.has_same_weights(dst, 1E-5));

    boost::filesystem::path tmp_path = boost::filesystem::unique_path();

    if (boost::filesystem::exists(tmp_path))
        throw nn_error("file exists");

    std::string tmp_file_path = tmp_path.string();

    // write
    {
        std::ofstream ofs(tmp_file_path.c_str());
        ofs << src;
    }

    // read
    {
        std::ifstream ifs(tmp_file_path.c_str());
        ifs >> dst;
    }

    boost::filesystem::remove(tmp_path); // remove temporary file

    EXPECT_TRUE(src.has_same_weights(dst, 1E-5));
}

TEST(read_write, fully_connected)
{
    fully_connected_layer<tan_h> l1(100, 100);
    fully_connected_layer<tan_h> l2(100, 100);

    l1.init_weight();
    l2.init_weight();

    serialization_test(l1, l2);
}

TEST(read_write, convolutional)
{
    typedef mse loss_func;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    convolutional_layer<tan_h> l1(5, 5, 3, 1, 1);
    convolutional_layer<tan_h> l2(5, 5, 3, 1, 1);

    l1.init_weight();
    l2.init_weight();

    serialization_test(l1, l2);
}

TEST(read_write, network)
{
    typedef mse loss_func;
    typedef network<loss_func, gradient_descent_levenberg_marquardt> network;

    network n1, n2;

    n1 << convolutional_layer<tan_h>(32, 32, 5, 1, 6) // C1, 1@32x32-in, 6@28x28-out
        << average_pooling_layer<tan_h>(28, 28, 6, 2) // S2, 6@28x28-in, 6@14x14-out
        << convolutional_layer<tan_h>(14, 14, 5, 6, 16) // C3, 6@14x14-in, 16@10x10-in
        << average_pooling_layer<tan_h>(10, 10, 16, 2) // S4, 16@10x10-in, 16@5x5-out
        << convolutional_layer<tan_h>(5, 5, 5, 16, 120) // C5, 16@5x5-in, 120@1x1-out
        << fully_connected_layer<tan_h>(120, 10); // F6, 120-in, 10-out

    n2 << convolutional_layer<tan_h>(32, 32, 5, 1, 6) // C1, 1@32x32-in, 6@28x28-out
        << average_pooling_layer<tan_h>(28, 28, 6, 2) // S2, 6@28x28-in, 6@14x14-out
        << convolutional_layer<tan_h>(14, 14, 5, 6, 16) // C3, 6@14x14-in, 16@10x10-in
        << average_pooling_layer<tan_h>(10, 10, 16, 2) // S4, 16@10x10-in, 16@5x5-out
        << convolutional_layer<tan_h>(5, 5, 5, 16, 120) // C5, 16@5x5-in, 120@1x1-out
        << fully_connected_layer<tan_h>(120, 10); // F6, 120-in, 10-out

    n1.init_weight();
    n2.init_weight();

    std::vector<vec_t> t;
    std::vector<label_t> l;
    t.push_back(vec_t(32*32, 0.0));
    l.push_back(3);
    n1.train(t, l, 1, 1);

    serialization_test(n1, n2);

    vec_t in(32*32, 0.0);

    auto res1 = n1.predict(in);
    auto res2 = n2.predict(in);

    for (int i = 0; i < 10; i++) {
        tiny_cnn::float_t eps = std::abs(res1[i]) * 1e-5;
        ASSERT_TRUE(std::abs(res1[i] - res2[i]) < eps);
    }
}

int main(void) {
    RUN_ALL_TESTS();
}


#include <boost/python.hpp>
#include <boost/assert.hpp>
#include <vector>
#include <Eigen/Dense>

#include "eigen_spline.hpp"

using namespace spline_planner;

namespace py = boost::python;

py::list ColumnMajorMatrixToRowMajorPython2DList(const Eigen::MatrixXd& mat) {
  py::list result;
  const size_t cols = mat.cols();
  const size_t rows = mat.rows();
  for (size_t i = 0; i < cols; ++i) {
    py::list vec;
    for (size_t j = 0; j < rows; ++j) {
      vec.append(mat(j, i));
    }
    result.append(vec);
  }
  return result;
}

Eigen::MatrixXd RowMajorPython2DListToColumnMajorMatrix(py::list list) {
  const size_t rows = py::len(list);
  const size_t cols = py::len(list[0]);
  Eigen::MatrixXd mat(rows, cols);
  for (size_t r = 0; r < rows; ++r) {
    py::list row = static_cast<py::list>(list[r]);
    for (size_t c = 0; c < cols; ++c) {
      mat(c, r) = py::extract<double>(row[c]);
    }
  }
  return mat;
}

struct SplineSamplingCallback {

  void operator()(double t, double s, const Eigen::MatrixXd& derivatives) {
    py::list point;
    point.append(t);
    point.append(s);
    point.append(ColumnMajorMatrixToRowMajorPython2DList(derivatives));
    result.append(point);
  }

  py::list result;

};

typedef spline_planner::Spline3D<3> CubicSpline3D;

struct CubicSpline3DWrapper {

  CubicSpline3DWrapper(py::list points)
    : spline_(nullptr) {
    const int num_pts = py::len(points);
    Eigen::MatrixXd mat(3, num_pts);
    for (size_t i = 0; i < num_pts; ++i) {
      mat(0, i) = py::extract<double>(points[i][0]);
      mat(1, i) = py::extract<double>(points[i][1]);
      mat(2, i) = py::extract<double>(points[i][2]);
    }
    spline_ = new CubicSpline3D(mat);
  }

  CubicSpline3DWrapper(py::list points, py::list derivatives, py::list index)
    : spline_(nullptr) {
    const int num_pts = py::len(points);
    Eigen::MatrixXd mat(3, num_pts);
    for (size_t i = 0; i < num_pts; ++i) {
      mat(0, i) = py::extract<double>(points[i][0]);
      mat(1, i) = py::extract<double>(points[i][1]);
      mat(2, i) = py::extract<double>(points[i][2]);
    }

    const int num_derivs = py::len(derivatives);
    Eigen::MatrixXd mat_derivs(3, num_derivs);
    for (size_t i = 0; i < num_derivs; ++i) {
      mat_derivs(0, i) = py::extract<double>(derivatives[i][0]);
      mat_derivs(1, i) = py::extract<double>(derivatives[i][1]);
      mat_derivs(2, i) = py::extract<double>(derivatives[i][2]);
    }

    Eigen::VectorXi vec_index(num_derivs);
    for (size_t i = 0; i < num_derivs; ++i) {
      vec_index(i) = py::extract<int>(index[i]);
    }

    spline_ = new CubicSpline3D(mat, mat_derivs, vec_index);
  }

  py::list SampleAndCollect0() {
    return SampleAndCollect();
  }

  py::list SampleAndCollect1(double sample_ds) {
    return SampleAndCollect(sample_ds);
  }

  py::list SampleAndCollect2(double sample_ds, double s_limit) {
    return SampleAndCollect(sample_ds, s_limit);
  }

  py::list SampleAndCollect(double sample_ds = 0.1, double s_limit=100.0, double dt = 0.01) {
    SplineSamplingCallback callback;
    spline_->Sample(callback, sample_ds, s_limit, 2, dt);
    return callback.result;
  }

  ~CubicSpline3DWrapper() {
    if (spline_) {
      delete spline_;
    }
  }

  CubicSpline3D* spline_;
};

BOOST_PYTHON_MODULE(eigen_spline) {

  py::class_<CubicSpline3DWrapper>("CubicSpline3DWrapper", py::init<py::list>())
    .def(py::init<py::list, py::list, py::list>())
    .def("sampleAndCollect", &CubicSpline3DWrapper::SampleAndCollect0)
    .def("sampleAndCollect", &CubicSpline3DWrapper::SampleAndCollect1)
    .def("sampleAndCollect", &CubicSpline3DWrapper::SampleAndCollect2)
    .def("sampleAndCollect", &CubicSpline3DWrapper::SampleAndCollect);

}


#include "FunctionList.h"
#include "exceptions/UnknownFunctionException.h"
#include <boost/assign/list_of.hpp>
#include <algorithm>

using namespace thewizardplusplus::wizard_basic_2::compiler::translator;
using namespace thewizardplusplus::wizard_basic_2::compiler::translator::
	exceptions;
using namespace boost::assign;

FunctionList::FunctionList(void) {
	functions = list_of
		// math module
		(Function(ValueType::NUMBER, "SIN", "std::sin", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "COS", "std::cos", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "TAN", "std::tan", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "ATN", "std::atan", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "EXP", "std::exp", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "LOG", "std::log", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "SQR", "std::sqrt", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "ABS", "std::abs", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "INT", "std::floor", list_of
			(FunctionParameter(ValueType::NUMBER))))
		// system module
		(Function(ValueType::NUMBER, "RND", "SystemModule::getInstance()."
			"getRandomNumber"))
		(Function(ValueType::NUMBER, "TIMER", "SystemModule::getInstance()."
			"getTimeFromStartInS"))
		(Function(ValueType::VOID, "PRINT", "SystemModule::getInstance().trace",
			list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "PRINT", "SystemModule::getInstance().trace",
			list_of
			(FunctionParameter(ValueType::ARRAY))))
		(Function(ValueType::NUMBER, "OPEN", "SystemModule::getInstance()."
			"fileOpen", list_of
			(FunctionParameter(ValueType::ARRAY))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "INPUT", "SystemModule::getInstance()."
			"fileInput", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "PRINT", "SystemModule::getInstance()."
			"fileOutput", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "PRINT", "SystemModule::getInstance()."
			"fileOutput", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::ARRAY))))
		(Function(ValueType::VOID, "CLOSE", "SystemModule::getInstance()."
			"fileClose", list_of
			(FunctionParameter(ValueType::NUMBER))))
		// graphics module
		(Function(ValueType::NUMBER, "SCREEN_WIDTH", "GraphicsModule::"
			"getInstance().getScreenWidth"))
		(Function(ValueType::NUMBER, "SCREEN_HEIGHT", "GraphicsModule::"
			"getInstance().getScreenHeight"))
		(Function(ValueType::VOID, "POSITION_CAMERA", "GraphicsModule::"
			"getInstance().setCameraPosition", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "ROTATION_CAMERA", "GraphicsModule::"
			"getInstance().setCameraRotation", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "FOG", "GraphicsModule::getInstance()."
			"setFogMode", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "FOG_COLOR", "GraphicsModule::getInstance()."
			"setFogColor", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "FOG_DEPTH", "GraphicsModule::getInstance()."
			"setFogDepth", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "LOAD_OBJECT", "GraphicsModule::"
			"getInstance().loadObject", list_of
			(FunctionParameter(ValueType::ARRAY))))
		(Function(ValueType::VOID, "POSITION_OBJECT", "GraphicsModule::"
			"getInstance().setObjectPosition", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "ROTATION_OBJECT", "GraphicsModule::"
			"getInstance().setObjectRotation", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "SCALE_OBJECT", "GraphicsModule::"
			"getInstance().setObjectScale", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "PLAY_OBJECT", "GraphicsModule::"
			"getInstance().playObjectAnimation", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "STOP_OBJECT", "GraphicsModule::"
			"getInstance().pauseObjectAnimation", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "KEYSTATE", "GraphicsModule::getInstance()"
			".isKeyPressed", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::NUMBER, "MOUSECLICK", "GraphicsModule::"
			"getInstance().getButtons"))
		(Function(ValueType::NUMBER, "MOUSEX", "GraphicsModule::getInstance()."
			"getPointerPositionX"))
		(Function(ValueType::NUMBER, "MOUSEY", "GraphicsModule::getInstance()."
			"getPointerPositionY"))
		(Function(ValueType::VOID, "POSITION_MOUSE", "GraphicsModule::"
			"getInstance().setPointerPosition", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "SYNC", "GraphicsModule::getInstance()."
			"update"))
		// sound module
		(Function(ValueType::NUMBER, "LOAD_SOUND", "SoundModule::getInstance()."
			"loadSound", list_of
			(FunctionParameter(ValueType::ARRAY))))
		(Function(ValueType::VOID, "PLAY_SOUND", "SoundModule::getInstance()."
			"playSound", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "STOP_SOUND", "SoundModule::getInstance()."
			"stopSound", list_of
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "POSITION_SOUND", "SoundModule::"
			"getInstance().setSoundPosition", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "POSITION_LISTENER", "SoundModule::"
			"getInstance().setListenerPosition", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))))
		(Function(ValueType::VOID, "ROTATION_LISTENER", "SoundModule::"
			"getInstance().setListenerRotation", list_of
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))
			(FunctionParameter(ValueType::NUMBER))));
}

Function FunctionList::getFunctionBySample(const Function& sample_function)
	const
{
	SimpleFunctionList::const_iterator end = functions.end();
	SimpleFunctionList::const_iterator finded = std::find(functions.begin(),
		end, sample_function);
	if (finded != end) {
		Function finded_function = *finded;
		finded_function.getParameters() = sample_function.getParameters();
		return finded_function;
	} else {
		SimpleFunctionList finded_functions;
		SimpleFunctionList::const_iterator i = functions.begin();
		for (; i != functions.end(); ++i) {
			Function function = *i;
			if (function.getName() == sample_function.getName()) {
				finded_functions.push_back(function);
			}
		}

		throw UnknownFunctionException(sample_function, finded_functions);
	}
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/hmsb.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/function/shr.hpp>

STF_CASE_TPL (" hmsb real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::hmsb;

  // specific values tests
  STF_EQUAL(hmsb(bs::Allbits<T>()), 1u);
  STF_EQUAL(hmsb(bs::Inf<T>()), 0u);
  STF_EQUAL(hmsb(bs::Minf<T>()), 1u);
  STF_EQUAL(hmsb(bs::Mone<T>()), 1u);
  STF_EQUAL(hmsb(bs::Nan<T>()), 1u);
  STF_EQUAL(hmsb(bs::One<T>()), 0u);
  STF_EQUAL(hmsb(bs::Signmask<T>()), 1u);
  STF_EQUAL(hmsb(bs::Zero<T>()), 0u);
} // end of test for real_

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
//


#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <wrapped_eigen.d/dense.d/denseMatrix.hpp>
#include <wrapped_eigen.d/sparse.d/sparseMatrix.hpp>
#include <wrapped_eigen.d/factorization.d/denseFactorizationFactory.hpp>

// Copies a C++ vector into a python list container
template <class T>
boost::python::list vectorToPythonList(std::vector<T> vector)
{
  typename std::vector<T>::iterator iter;
  boost::python::list list;
  for (iter = vector.begin(); iter != vector.end(); ++iter) {
      list.append(*iter);
  }
  return list;
}

// Converts a denseMatrix class to a python list
template <class T>
boost::python::list denseMatrixToPythonList(denseMatrix<T> matrix)
{
  boost::python::list list;
  std::vector<T> data = matrix.container();
  list = vectorToPythonList(data);
  return list;
}

// python wrapper of libpeigen module
BOOST_PYTHON_MODULE(libpeigen)
{
  using namespace boost::python;

  def("doubleVecToList", &vectorToPythonList<double>);

  def("denseMatrixToList", &denseMatrixToPythonList<double>);

  class_<std::vector<double> >("doubleVec")
    .def(vector_indexing_suite<std::vector<double> >())
  ;

  class_<std::vector<float> >("floatVec")
    .def(vector_indexing_suite<std::vector<float> >())
  ;

  typedef denseMatrix<double> dMDouble;
  class_<dMDouble>("denseMatrixDouble")
    .def(init< >())
    .def(init<int,int>())
    .def(init<std::vector<double>&,int,int>())
    .def("resize", &dMDouble::resize)
    .def("setRandom", &dMDouble::setRandom)
    .def("assign", &dMDouble::assign)  // operator= not accessable in python
    .def("setElem", &dMDouble::setElem)
    .def("getElem", &dMDouble::getElem)
    .def("norm", &dMDouble::norm)
    .def("trace", &dMDouble::trace)
    .def("transpose",  &dMDouble::transpose)
    .def(self += self)
    .def(self *= int())
	.def(self *= float())
    .def(self + self)
    .def(self - self)
    .def(self * self)
    .def("show", &dMDouble::print) // print() is a reserved sytax in python
    .def("rows", &dMDouble::get_rows)
    .def("row", &dMDouble::get_row)
    .def("cols", &dMDouble::get_cols)
    .def("col", &dMDouble::get_col)
    .def("diagonal", &dMDouble::get_diagonal)
    .def("block", &dMDouble::get_block)
    .def("setRow", &dMDouble::set_row)
    .def("setCol", &dMDouble::set_col)
    .def("setDiagonal", &dMDouble::set_diagonal)
    .def("setBlock", &dMDouble::set_block)
    .def("save", &dMDouble::save)
    .def("load", &dMDouble::load)
  ;

  typedef sparseMatrix<double> sMDouble;
  class_<sMDouble>("sparseMatrixDouble")
    .def(init< >())
    .def(init<int,int>())
    .def(init<int,int,int>())
    .def(init<std::vector<double>&,std::vector<int>&,std::vector<int>&,int,int>())
    .def("resize", &sMDouble::resize)
    .def("reserve", &sMDouble::reserve)
    .def("assign", &sMDouble::assign)  // operator= not accessable in python
    .def("setElem", &sMDouble::setElem) //
    .def(self += self)
    .def(self *= int())
    .def(self + self)
    .def(self * self)
    .def("show", &sMDouble::print) // print() is a reserved sytax in python
    .def("save", &sMDouble::save)
    .def("load", &sMDouble::load)
  ;

  typedef denseFactorizationFactory<dMDouble> dFFactory;
  class_<dFFactory>("denseDecomposition")
    .def(init< >())
    .def(init<dMDouble&>())
    .def("reset", &dFFactory::reset)
    .def("BDCSVD", &dFFactory::BDCSVD)
    .def("HouseholderQR", &dFFactory::HouseholderQR)
	.def("PartialPivLU", &dFFactory::PartialPivLU)
    .def("getU", &dFFactory::getU)
    .def("getSingularValues", &dFFactory::getSingularValues)
    .def("getV", &dFFactory::getV)
    .def("getQ", &dFFactory::getQ)
  ;
}


// Copyright 2019 Fred Hutchinson Cancer Research Center
// See the included LICENSE file for details on the licence that is granted to the user of this software.
#include <cytolib/calibrationTable.hpp>
#include <boost/config.hpp>
#include <boost/foreach.hpp>

namespace cytolib
{

	vector<double> calibrationTable::getX(){return x;};
	vector<double> calibrationTable::getY(){return y;};
	void calibrationTable::setY(vector<double> _y){
			y=_y;
			};
	void calibrationTable::setX(vector<double> _x){
				x=_x;
				};
	vector<double> calibrationTable::getB(){return b;};
	vector<double> calibrationTable::getC(){return c;};
	vector<double> calibrationTable::getD(){return d;};
	void calibrationTable::setCaltype(string _caltype){caltype=_caltype;};
	string calibrationTable::getCaltype(){return caltype;};
	void calibrationTable::setMethod(int _spline_method){spline_method=_spline_method;};
	int calibrationTable::getMethod(){return spline_method;};
	void calibrationTable::setInterpolated(bool _flag){flag=_flag;};
	bool calibrationTable::isInterpolated(){return flag;}
	calibrationTable::calibrationTable(){
		flag=false;
	}

	calibrationTable::calibrationTable(string _caltype,int _spline_method){
	//								type=CALTBL;
			caltype=_caltype;
			spline_method=_spline_method;
			flag=false;
	}

	void calibrationTable::interpolate(){

	//	PRINT("entering interpolate\n");

		if(!flag)
		{
			b.resize(x.size());
			c.resize(x.size());
			d.resize(x.size());
			natural_spline(x, y, b, c, d);
			flag=true;
		}

	}
	/*
	 * API provided for Rcpp to access calibration table
	 */
	Spline_Coefs calibrationTable::getSplineCoefs(){

		map<string,vector<double> > coefs;

		coefs["x"]=x;
		coefs["y"]=y;
		coefs["b"]=b;
		coefs["c"]=c;
		coefs["d"]=d;

		Spline_Coefs res;
		res.coefs=coefs;
		res.method=spline_method;
		res.type=caltype;

		return res;
	}
	void calibrationTable::transforming(double * input, int nSize){

		int imeth=2;

		spline_eval(imeth,input, nSize, x, y, b, c, d);

	}

	void calibrationTable::convertToPb(pb::calibrationTable & cal_pb){
		if(!isInterpolated())
			interpolate();
		for(unsigned i = 0; i < x.size(); i++){
			cal_pb.add_x(x[i]);
			cal_pb.add_y(y[i]);
			cal_pb.add_b(b[i]);
			cal_pb.add_c(c[i]);
			cal_pb.add_d(d[i]);
		}
		cal_pb.set_spline_method(spline_method);
		cal_pb.set_caltype(caltype);
		cal_pb.set_flag(flag);
	}

	calibrationTable::calibrationTable(const pb::calibrationTable & cal_pb){
		int nSize = cal_pb.x_size();
		x.resize(nSize);
		y.resize(nSize);
		b.resize(nSize);
		c.resize(nSize);
		d.resize(nSize);
		for(int i = 0; i < nSize; i++){
			x[i] = cal_pb.x(i);
			y[i] = cal_pb.y(i);
			b[i] = cal_pb.b(i);
			c[i] = cal_pb.c(i);
			d[i] = cal_pb.d(i);
		}
		spline_method = cal_pb.spline_method();
		caltype = cal_pb.caltype();
		flag = cal_pb.flag();
	}

};


#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Dense>
#include <boost/format.hpp>
#include <cmath>
#include <gtest/gtest.h>
#include <iostream>
#include <sstream>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/expr_op_overloads.hpp>
#include <trajopt_sco/modeling_utils.hpp>
#include <trajopt_sco/optimizers.hpp>
#include <trajopt_sco/sco_common.hpp>
#include <trajopt_sco/solver_interface.hpp>
#include <trajopt_utils/logging.hpp>
#include <trajopt_utils/stl_to_string.hpp>

using namespace util;
using namespace std;
using namespace sco;
using namespace Eigen;

class SQP : public testing::TestWithParam<ModelType>
{
protected:
  SQP()
  {
  }
};

void setupProblem(OptProbPtr& probptr, size_t nvars, ModelType convex_solver)
{
  probptr.reset(new OptProb(convex_solver));
  vector<string> var_names;
  for (size_t i = 0; i < nvars; ++i)
  {
    var_names.push_back((boost::format("x_%i") % i).str());
  }
  probptr->createVariables(var_names);
}

void expectAllNear(const DblVec& x, const DblVec& y, double abstol)
{
  EXPECT_EQ(x.size(), y.size());
  stringstream ss;
  LOG_INFO("checking %s ?= %s", CSTR(x), CSTR(y));
  for (size_t i = 0; i < x.size(); ++i)
    EXPECT_NEAR(x[i], y[i], abstol);
}

double f_QuadraticSeparable(const VectorXd& x)
{
  return x(0) * x(0) + sq(x(1) - 1) + sq(x(2) - 2);
}
TEST_P(SQP, QuadraticSeparable)
{
  // if the problem is exactly a QP, it should be solved in one iteration
  OptProbPtr prob;
  setupProblem(prob, 3, GetParam());
  prob->addCost(CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticSeparable), prob->getVars(), "f")));
  BasicTrustRegionSQP solver(prob);
  BasicTrustRegionSQPParameters& params = solver.getParameters();
  params.trust_box_size = 100;
  DblVec x = { 3, 4, 5 };
  solver.initialize(x);
  OptStatus status = solver.optimize();
  ASSERT_EQ(status, OPT_CONVERGED);
  expectAllNear(solver.x(), { 0, 1, 2 }, 1e-3);
  // todo: checks on number of iterations and function evaluates
}
double f_QuadraticNonseparable(const VectorXd& x)
{
  return sq(x(0) - x(1) + 3 * x(2)) + sq(x(0) - 1) + sq(x(2) - 2);
}
TEST_P(SQP, QuadraticNonseparable)
{
  OptProbPtr prob;
  setupProblem(prob, 3, GetParam());
  prob->addCost(
      CostPtr(new CostFromFunc(ScalarOfVector::construct(&f_QuadraticNonseparable), prob->getVars(), "f", true)));
  BasicTrustRegionSQP solver(prob);
  BasicTrustRegionSQPParameters& params = solver.getParameters();
  params.trust_box_size = 100;
  params.min_trust_box_size = 1e-5;
  params.min_approx_improve = 1e-6;
  DblVec x = { 3, 4, 5 };
  solver.initialize(x);
  OptStatus status = solver.optimize();
  ASSERT_EQ(status, OPT_CONVERGED);
  expectAllNear(solver.x(), { 1, 7, 2 }, .01);
  // todo: checks on number of iterations and function evaluates
}

void testProblem(ScalarOfVectorPtr f, VectorOfVectorPtr g, ConstraintType cnt_type, const DblVec& init,
                 const DblVec& sol, ModelType convex_solver)
{
  OptProbPtr prob;
  size_t n = init.size();
  assert(sol.size() == n);
  setupProblem(prob, n, convex_solver);
  prob->addCost(CostPtr(new CostFromFunc(f, prob->getVars(), "f", true)));
  prob->addConstraint(ConstraintPtr(new ConstraintFromErrFunc(g, prob->getVars(), VectorXd(), cnt_type, "g")));
  BasicTrustRegionSQP solver(prob);
  BasicTrustRegionSQPParameters& params = solver.getParameters();
  params.max_iter = 1000;
  params.min_trust_box_size = 1e-5;
  params.min_approx_improve = 1e-10;
  params.merit_error_coeff = 1;

  solver.initialize(init);
  OptStatus status = solver.optimize();
  EXPECT_EQ(status, OPT_CONVERGED);
  expectAllNear(solver.x(), sol, .01);
}
// http://www.ai7.uni-bayreuth.de/test_problem_coll.pdf

double f_TP1(const VectorXd& x)
{
  return 1 * sq(x(1) - sq(x(0))) + sq(1 - x(0));
}
VectorXd g_TP1(const VectorXd& x)
{
  VectorXd out(1);
  out(0) = -1.5 - x(1);
  return out;
}
double f_TP2(const VectorXd& x)
{
  return 100 * sq(x(1) - sq(x(0))) + sq(1 - x(0));
}
VectorXd g_TP2(const VectorXd& x)
{
  VectorXd out(1);
  out(0) = -1.5 - x(1);
  return out;
}
double f_TP3(const VectorXd& x)
{
  return (x(1) + 1e-5 * sq(x(1) - x(0)));
}
VectorXd g_TP3(const VectorXd& x)
{
  VectorXd out(1);
  out(0) = 0 - x(1);
  return out;
}
double f_TP6(const VectorXd& x)
{
  return sq(1 - x(0));
}
VectorXd g_TP6(const VectorXd& x)
{
  VectorXd out(1);
  out(0) = 10 * (x(1) - sq(x(0)));
  return out;
}
double f_TP7(const VectorXd& x)
{
  return log(1 + sq(x(0))) - x(1);
}
VectorXd g_TP7(const VectorXd& x)
{
  VectorXd out(1);
  out(0) = sq(1 + sq(x(0))) + sq(x(1)) - 4;
  return out;
}

TEST_P(SQP, TP1)
{
  testProblem(ScalarOfVector::construct(&f_TP1), VectorOfVector::construct(&g_TP1), INEQ, { -2, 1 }, { 1, 1 },
              GetParam());
}
TEST_P(SQP, TP3)
{
  testProblem(ScalarOfVector::construct(&f_TP3), VectorOfVector::construct(&g_TP3), INEQ, { 10, 1 }, { 0, 0 },
              GetParam());
}
TEST_P(SQP, TP6)
{
  testProblem(ScalarOfVector::construct(&f_TP6), VectorOfVector::construct(&g_TP6), EQ, { 10, 1 }, { 1, 1 },
              GetParam());
}
TEST_P(SQP, TP7)
{
  testProblem(ScalarOfVector::construct(&f_TP7), VectorOfVector::construct(&g_TP7), EQ, { 2, 2 }, { 0., sqrtf(3.) },
              GetParam());
}

INSTANTIATE_TEST_CASE_P(AllSolvers, SQP, testing::ValuesIn(availableSolvers()));


#include <boost/integer/integer_log2.hpp>


#include <boost/ut.hpp>
#include <cinttypes>
#include <libxbitset/bitset.hpp>

using namespace boost::ut;

struct Register
{
  volatile uint64_t CTRL1;
  volatile uint64_t CTRL2;
};

Register memory{};
Register* reg = &memory;

int main()
{
  static constexpr auto intensity =
    xstd::bitrange{ .position = 16, .width = 8 };
  static constexpr auto clock_divider = xstd::bitrange::from<42, 50>();
  xstd::bitmanip(reg->CTRL1)
    .insert<intensity>(7)
    .set(4)
    .reset(0)
    .insert<clock_divider>(3);

  expect(that % 0x0000'0C00'0007'0010 == reg->CTRL1);
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// 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)
//[ transform_terminals
#include <boost/yap/yap.hpp>

struct iota_terminal_transform
{
    // Base case. Note that we're not treating placeholders specially for this
    // example (they're easy to special-case if necessary).
    template<typename T>
    auto operator()(boost::yap::expr_tag<boost::yap::expr_kind::terminal>, T && t)
    {
        // Like the std::iota() algorithm, we create replacement int terminals
        // with the values index_, index_ + 1, index_ + 2, etc.
        return boost::yap::make_terminal(index_++);
    }

    // Recursive case: Match any call expression.
    template<typename CallableExpr, typename... Arg>
    auto operator()(boost::yap::expr_tag<boost::yap::expr_kind::call>,
                    CallableExpr callable, Arg &&... arg)
    {
        // Even though the callable in a call expression is technically a
        // terminal, it doesn't make a lot of sense to replace it with an int,
        // so we'll only transform the argument subexpressions.
        return boost::yap::make_expression<boost::yap::expr_kind::call>(
            boost::yap::as_expr(callable),
            boost::yap::transform(boost::yap::as_expr(arg), *this)...);
    }

    int index_;
};

int sum(int a, int b) { return a + b; }

int main()
{
    {
        // This simple sum(8, 8) expression requires both overloads of
        // iota_terminal_transform.
        auto expr = boost::yap::make_terminal(sum)(8, 8);
        assert(evaluate(expr) == 16);

        auto iota_expr = boost::yap::transform(expr, iota_terminal_transform{1});
        assert(evaluate(iota_expr) == 3);
    }

    {
        // This expression requires only the terminal case of
        // iota_terminal_transform.
        auto expr = -(boost::yap::make_terminal(8) + 8);
        assert(evaluate(expr) == -16);

        auto iota_expr = boost::yap::transform(expr, iota_terminal_transform{0});
        assert(evaluate(iota_expr) == -1);
    }

    {
        // Like the first expression above, this expression requires both
        // overloads of iota_terminal_transform.
        auto expr = boost::yap::make_terminal(sum)(-(boost::yap::make_terminal(8) + 8), 0);
        assert(evaluate(expr) == -16);

        auto iota_expr = boost::yap::transform(expr, iota_terminal_transform{0});
        assert(evaluate(iota_expr) == -3);
    }
}
//]


//
// Add --log_level=message to see the messages!
//
#define BOOST_TEST_MODULE "Custom NeuralNetwork Tests"
#include <boost/test/unit_test.hpp>

#include <sstream>

#include "core/types.h"
#include "core/functions.h"
#include "core/utils.h"
#include "core/random.h"
#include "core/updaters.h"
#include "core/training.h"
#include "core/nn.h"
#include "layers/contlayer.h"
#include "layers/convlayer.h"
#include "layers/fclayer.h"
#include "layers/polllayer.h"
#include "layers/smaxlayer.h"

#include "test_utils.h"
#include "timer.h"
#include "mnist_test.h"

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace yann;
using namespace yann::test;

#define MNIST_TEST_FOLDER   "../data/mnist/"

struct CustomNNTestFixture
{
  MnistTest _mnist_test;

  CustomNNTestFixture()
  {
    BOOST_TEST_MESSAGE("*** Reading MNIST dataset ...");
    _mnist_test.read(MNIST_TEST_FOLDER);
    BOOST_TEST_MESSAGE(_mnist_test);
  }
  ~CustomNNTestFixture()
  {
  }

}; // struct CustomNNTestFixture

BOOST_FIXTURE_TEST_SUITE(CustomNNTest, CustomNNTestFixture);

BOOST_AUTO_TEST_CASE(Test, * disabled())
{
  const auto input_rows  = _mnist_test.get_image_rows();
  const auto input_cols  = _mnist_test.get_image_cols();
  const auto output_size = _mnist_test.get_label_size();
  const size_t training_batch_size = 10;
  const size_t testing_batch_size = 100;
  const size_t epochs = 100;
  const double learning_rate = 0.75;
  const double regularization = 0.001;

  auto create_one_path = [&]() {
    auto poll_layer = make_unique<PollingLayer>(input_rows, input_cols, 2, PollingLayer::PollMode_Avg);
    YANN_CHECK(poll_layer);
    poll_layer->set_activation_function(make_unique<SigmoidFunction>());

    auto fc1_layer = make_unique<FullyConnectedLayer>(
        poll_layer->get_output_size(),
        300);
    YANN_CHECK(fc1_layer);
    fc1_layer->set_activation_function(make_unique<SigmoidFunction>());

    auto fc2_layer = make_unique<FullyConnectedLayer>(
        fc1_layer->get_output_size(),
        output_size);
    YANN_CHECK(fc2_layer);
    fc2_layer->set_activation_function(make_unique<SigmoidFunction>());

    // add to container
    auto container = make_unique<SequentialLayer>();
    YANN_CHECK(container);
    container->append_layer(std::move(poll_layer));
    container->append_layer(std::move(fc1_layer));
    container->append_layer(std::move(fc2_layer));

    return container;
  };
  auto container1 = create_one_path();
  auto container2 = create_one_path();

  auto conv_layer = make_unique<ConvolutionalLayer>(
      1,
      container1->get_output_size(),
      1);
  YANN_CHECK(conv_layer);
  conv_layer->set_activation_function(make_unique<SigmoidFunction>());

  auto bcast_layer = make_unique<BroadcastLayer>();
  bcast_layer->append_layer(std::move(container1));
  bcast_layer->append_layer(std::move(container2));

  auto merge_layer = make_unique<MergeLayer>(2);
  merge_layer->append_layer(std::move(conv_layer));

  auto container = make_unique<SequentialLayer>();
  YANN_CHECK(container);
  container->append_layer(std::move(bcast_layer));
  container->append_layer(std::move(merge_layer));

  // create the network
  auto nn = make_unique<Network>(std::move(container));
  YANN_CHECK(nn);
  nn->set_cost_function(make_unique<QuadraticCost>());
  nn->init(Layer::InitMode_Random);

  // Trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescent>(learning_rate, regularization));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" Network: " << nn->get_info());
  BOOST_TEST_MESSAGE(" Trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" Epochs: " << epochs);

  // train and test
  pair<double, Value> res = _mnist_test.train_and_test(
      *nn,
      *trainer,
      DataSource_Stochastic::Random,
      training_batch_size,
      epochs,
      testing_batch_size);
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100) << "% Loss: " << res.second << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" Network: " << nn->get_info());
  BOOST_TEST_MESSAGE(" Trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" Epochs: " << epochs);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Raimar Sandner 2012–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Particle.h"
#include "TMP_Tools.h"
#include "DensityOperator.h"
#include "LazyDensityOperator.h"
#include "LazyDensityOperatorFFT.h"

#include <iostream>
#include <boost/test/unit_test.hpp>

typedef quantumdata::StateVector<3>         SV3;
typedef quantumdata::DensityOperator<3>     DO3;
typedef quantumdata::LazyDensityOperator<3> LDO3;

const double eps=1e-12;

BOOST_AUTO_TEST_CASE( LAZY_DENSITY_OPERATOR_FFTRANSFORM )
{
  using tmptools::Vector;
  using namespace particle;
  using namespace quantumdata;
  using namespace blitz;
  using mathutils::fcmp;
  InitialCondition ic1(0.5,0,1,true);
  InitialCondition ic2(0.5,0,2,true);
  InitialCondition ic3(0.5,0,3,true);
  Spatial spacial(3);
  SV3 psi1 = wavePacket(ic1,spacial,true)*wavePacket(ic2,spacial,true)*wavePacket(ic3,spacial,true);
  SV3 psi2 = wavePacket(ic1,spacial,false)*wavePacket(ic2,spacial,true)*wavePacket(ic3,spacial,false);
  LDO3::Ptr psiX = ffTransform<Vector<0,2> >(psi1,fft::DIR_KX);
  const SV3 *psi3 = dynamic_cast<const SV3 *>(psiX.get());
  BOOST_CHECK(max(abs(psi2.getArray()-psi3->getArray()))<eps);

  DO3 rho1 = dyad(psi1,psi1);
  DO3 rho2 = dyad(psi2,psi2);
  LDO3::Ptr rhoX = ffTransform<Vector<0,2> >(rho1,fft::DIR_KX);
  const DO3 *rho3 = dynamic_cast<const DO3 *>(rhoX.get());
  BOOST_CHECK(max(abs(rho2.getArray()-rho3->getArray()))<eps);
}


/**
 * @file test_string_seq.cpp
 * @author Bensuperpc (bensuperpc@gmail.com)
 * @brief
 * @version 1.0.0
 * @date 2021-04-01
 *
 * MIT License
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_string_seq

#include <boost/predef.h>
#include <boost/test/unit_test.hpp>
#include "string_lib/string_lib_impl.hpp"

BOOST_AUTO_TEST_CASE(test_string_seq_0)
{
    const char alpha[27] = alphabetMax; // Get alphabetic seq
    char tmp1[2] = {0};
    char tmp2[2] = {'A', '\0'};
    for (std::size_t i = 1; i < 26; i++) {
        my::string::findStringInv<std::size_t>(i, tmp1);
        tmp2[0] = alpha[i - 1];
        BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
    }
}

BOOST_AUTO_TEST_CASE(test_string_seq_1)
{
    char tmp1[2] = {0};
    char tmp2[2] = {'A', '\0'};

    my::string::findStringInv<std::size_t>(1, tmp1);
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}

BOOST_AUTO_TEST_CASE(test_string_seq_2)
{
    char tmp1[2] = {0};
    char tmp2[2] = {'Z', '\0'};

    my::string::findStringInv<std::size_t>(26, tmp1);
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}

BOOST_AUTO_TEST_CASE(test_string_seq_3)
{
    char tmp1[3] = {0};
    const char tmp2[3] = {'A', 'A', '\0'};

    my::string::findStringInv<std::size_t>(27, tmp1);
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}

BOOST_AUTO_TEST_CASE(test_string_seq_4)
{
    char tmp1[3] = {0};
    const char tmp2[3] = {'Y', 'Y', '\0'};

    my::string::findStringInv<std::size_t>(675, tmp1);
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}

BOOST_AUTO_TEST_CASE(test_string_seq_5)
{
    char tmp1[3] = {0};
    const char tmp2[3] = {'Z', 'Z', '\0'};

    my::string::findStringInv<std::size_t>(702, tmp1); // 26^1 + 26^2
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}

BOOST_AUTO_TEST_CASE(test_string_seq_6)
{
    char tmp1[4] = {0};
    const char tmp2[4] = {'Z', 'Z', 'Z', '\0'};

    my::string::findStringInv<std::size_t>(18278, tmp1); // 26^1 + 26^2 + 26^3
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}

BOOST_AUTO_TEST_CASE(test_string_seq_7)
{
    char tmp1[5] = {0};
    const char tmp2[5] = {'Z', 'Z', 'Z', 'Z', '\0'};

    my::string::findStringInv<std::size_t>(475254, tmp1); // 26^1 + 26^2 + 26^3 + 26^4
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}

BOOST_AUTO_TEST_CASE(test_string_seq_8)
{
    char tmp1[6] = {0};
    const char tmp2[6] = {'A', 'A', 'A', 'A', 'A', '\0'};

    my::string::findStringInv<std::size_t>(475255, tmp1); // 26^1 + 26^2 + 26^3 + 26^4
    BOOST_REQUIRE(strcmp(tmp1, tmp2) == 0);
}


#include <gtest/gtest.h>

#include <cassert>

#include <vector>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <stdexcept>

// C++11
#include <unordered_map>

// no standart now
//#include <ext/hash_set>
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

// DANGER: Кажется с пользовательскими типами как-то не очень. Что то похоже и про строки слышал, кажется.
#include <google/dense_hash_map>

// 3rdpary
#include <boost/foreach.hpp>

// App
#include "details/io_details.h"
#include "details/fp_details.h"

using namespace std;

using std::hash;  // C++11
//using boost::hash;  // нужно точечно указать

// Precond.:
//  Keys is uniq.
//
int _2sum_naive(const vector<int>& in, const int target)
{
  int count_unique = 0;
  for (vector<int>::const_iterator at = in.begin(), end = in.end(); at != end; ++at) {

    // С границами что-то не то
    // Ищет только первый, как и нужно
    vector<int>::const_iterator finded_it = find_if(
	at, // begin  TODO: may be bug
	//in.begin()
	in.end(),
	fp::compose_f_gx(bind2nd(equal_to<int>(), target), bind2nd(plus<int>(), *at)));

    if (finded_it != in.end())
      count_unique++;
  }
  return count_unique;
}

int q1(const vector<int>& in) {
  // http://publib.boulder.ibm.com/infocenter/comphelp/v9v111/index.jsp?topic=/com.ibm.xlcpp9.aix.doc/standlib/stl_unordered_set.htm
  boost::unordered_set<int> htbl;
  int low = 2500;
  int high = 4000;

  // de-duplication
  BOOST_FOREACH(int val, in) {
    if (htbl.end() == htbl.find(val))
      if (val < high+1)  // можно ужать еще
	htbl.insert(val);
  };
  assert(htbl.size() < in.size());
  assert(!htbl.empty());

  int count_unique = 0;
  for (int t = low; t < high+1; ++t) {
    BOOST_FOREACH(int x, htbl) {
      int y = t - x;
      if (htbl.find(y) != htbl.end()) {
	if (x != y) {
	  count_unique++;
	  break;
	}
      }
    }
  }
  return count_unique;
}

long q2_nth(const vector<int>& in) {
  vector<int> in_stream = in;
  vector<int> cursor;
  long mean = 0;
  cursor.reserve(in.size());
  BOOST_FOREACH(int elem, in_stream) {
    cursor.push_back(elem);
    int size = cursor.size();
    int nth = 0;
    if (size % 2 == 1) {
      nth = (size + 1) / 2 - 1;
    } else {
      nth = size / 2 - 1;
    }

    // Возможно частичная сортировка
    assert(cursor.begin()+nth != cursor.end());
    nth_element(cursor.begin(), cursor.begin()+nth, cursor.end());
    mean += cursor[nth];
  }
  return mean;
}

TEST(W5_6, PQuestions) {
  /// Q1
  // Задание не понятно
  //int finded = _2sum_naive(in, target);
  //assert(finded > 0);
  // TODO: сделать все три варианта
  vector<int> in = io_details::extract_records("./input_data/HashInt.txt");
  int count_unique = q1(in);
  assert(count_unique > 0 && count_unique < 1501);
  assert(count_unique == 1477);
  assert(!in.empty());

  /// Q2
  in = io_details::extract_records("./input_data/Median.txt");
  // TODO: heap and bfs based impl.
  assert(q2_nth(in) % 10000 == 1213);
}


#include <string>

#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/program_options.hpp>
#include <boost/progress.hpp>

#include "point_defs.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

#include <octomap/octomap.h>
#include <octomap/Pointcloud.h>
#include <octomap/ColorOcTree.h>

#include "utils/path_utils.h"
#include "utils/cloud_utils.h"
#include "utils/hdf_utils.h"

#include "parameters.h"

namespace fs = boost::filesystem;

template <typename PointT>
void pcl_to_octomap(boost::shared_ptr<pcl::PointCloud<PointT> > src_cloud, octomap::Pointcloud& octomap_cloud)
{
    octomap_cloud.clear();
    octomap::point3d octopt;
    BOOST_FOREACH(PointT pt, src_cloud->points)
    {
        octopt(0) = pt.x;
        octopt(1) = pt.y;
        octopt(2) = pt.z;
        octomap_cloud.push_back(octopt);
    }
}

int main(int argc, char** argv)
{
    params().initialize();

    // Set up paths

    int count = params().count;

    std::string transforms_dir = params().h5_dir;

    std::vector<std::string> pcd_paths;
    // Read transforms
    std::vector<std::string> transform_paths;

    get_range_files(params().color_clouds_dir, 0, 1, count, "%1%.pcd", pcd_paths);
    get_range_files(transforms_dir, 0, 1, count, "%1%.transform", transform_paths);

    assert(pcd_paths.size() == transform_paths.size());

    // Transformations

    octomap::point3d sensor_origin;
    MatrixXfRowMajor transform;
    Eigen::Vector3f  init_pos;

    // Build the octomap

    boost::progress_display show_progress(pcd_paths.size());

    boost::shared_ptr<octomap::ColorOcTree> octree(new octomap::ColorOcTree(params().color_octree_res));
    octree->setProbHit(params().prob_hit);
    octree->setProbMiss(params().prob_miss);
    octree->setClampingThresMax(params().clamping_thres_max);
    octree->setClampingThresMin(params().clamping_thres_min);
    boost::shared_ptr<octomap::ColorOcTree> octree_centered(new octomap::ColorOcTree(*octree));

    for (int k = 0; k < pcd_paths.size(); k++)
    {
        // Load point cloud and transforms

        std::string pcd_path = pcd_paths[k];
        std::string transform_path = transform_paths[k];

        if (!fs::exists(transform_path))
        {
            std::cout << transform_path << " does not exist, quitting" << std::endl;
            break;
        }

        H5::H5File transform_file(transform_path, H5F_ACC_RDONLY);
        load_hdf_dataset(transform_file, "transform", transform, H5::PredType::NATIVE_FLOAT);
        Eigen::Matrix4f transform_copy(transform);

        Eigen::Vector4f imu_origin = transform_copy.block<4, 1>(0, 3);
        Eigen::Vector4f lidar_origin = params().T_from_i_to_l * imu_origin;

        sensor_origin(0) = lidar_origin(0);
        sensor_origin(1) = lidar_origin(1);
        sensor_origin(2) = lidar_origin(2);

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
        load_cloud(pcd_path, src_cloud);

        octomap::Pointcloud octomap_cloud;
        pcl_to_octomap(src_cloud, octomap_cloud);
        octree->insertPointCloud(octomap_cloud, sensor_origin);
        BOOST_FOREACH(pcl::PointXYZRGB pt, src_cloud->points)
        {
            octomap::ColorOcTreeNode* node = octree->search(pt.x, pt.y, pt.z);
            if (node)
                node->setColor(pt.r, pt.g, pt.b);
        }

        // Following is for octovis so the map is close to centered
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
        if (k == 0)
            init_pos = transform_copy.block<3, 1>(0, 3);
        sensor_origin(0) -= init_pos(0);
        sensor_origin(1) -= init_pos(1);
        sensor_origin(2) -= init_pos(2);
        Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
        T(0, 3) -= init_pos(0);
        T(1, 3) -= init_pos(1);
        T(2, 3) -= init_pos(2);
        pcl::transformPointCloud(*src_cloud, *centered_cloud, T);

        pcl_to_octomap(centered_cloud, octomap_cloud);
        octree_centered->insertPointCloud(octomap_cloud, sensor_origin);
        BOOST_FOREACH(pcl::PointXYZRGB pt, centered_cloud->points)
        {
            octomap::ColorOcTreeNode* node = octree_centered->search(pt.x, pt.y, pt.z);
            if (node)
                node->setColor(pt.r, pt.g, pt.b);
        }

        ++show_progress;
    }

    std::cout << "Writing octree of size " << octree->size() << std::endl;
    octree->write(params().color_octomap_file);
    octree_centered->writeBinary(params().centered_color_octomap_file);

    return 0;
}


/*
 *  testPermittivitySensitivity.cpp
 *  Trogdor6
 *
 *  Created by Paul Hansen on 9/11/10.
 *  Copyright 2010 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestPermittivitySensitivity

#include <boost/test/unit_test.hpp>

#include "TensorGridConstants.h"

#include <iomanip>

using namespace std;
using namespace RLE;
using namespace YeeUtilities;

Matrix3d sBigDot(const Matrix3<Vector3d> & lhs, const Vector3d & rhs)
{
    return Matrix3d(
        dot(lhs[0], rhs), dot(lhs[1], rhs), dot(lhs[2], rhs),
        dot(lhs[3], rhs), dot(lhs[4], rhs), dot(lhs[5], rhs),
        dot(lhs[6], rhs), dot(lhs[7], rhs), dot(lhs[8], rhs));
}

BOOST_AUTO_TEST_CASE( TestDiagonalSensitivity )
{
    const int WHATEVER = 0;
    DynamicRLE3<Precision::RationalFunction> WHO_CARES;
    TensorGridConstants tgc(WHATEVER, WHATEVER, WHO_CARES, Rect3i(0,0,0,0,0,0));

    vector<Precision::RationalFunction> materials;
    materials.push_back(Precision::RationalFunction(1.0));
    materials.push_back(Precision::RationalFunction(2.0));

    vector<DynamicRLE3<double> > fillFactors0(2, DynamicRLE3<double>(0,0,0)),
        fillFactors1(2, DynamicRLE3<double>(0,0,0));
    DynamicRLE3<double> orientations0(0,0,0), orientations1(0,0,0);
    vector<DynamicRLE3<double> > fillFactorSensitivities(2, DynamicRLE3<double>(0,0,0));
    DynamicRLE3<double> orientationSensitivities(0,0,0);

    fillFactors0[0].mark(0,0,0,0.75);
    fillFactors0[1].mark(0,0,0,0.25);
    fillFactors1[0].mark(0,0,0,0.76);
    fillFactors1[1].mark(0,0,0,0.24);

    orientations0.mark(0,0,0,0.5);
    orientations1.mark(0,0,0,0.6);

    fillFactorSensitivities[0].mark(0,0,0,0.01); // durr.  just to be sure.
    fillFactorSensitivities[1].mark(0,0,0,-0.01);

    orientationSensitivities.mark(0,0,0,0.1);

    Pointer<DynamicRLE3<Precision::RationalFunction> > dPermittivity =
        tgc.diagonalSensitivity(materials, fillFactors0,
        orientations0, fillFactorSensitivities, orientationSensitivities);

    Precision::RationalFunction xsi0 =
        tgc.diagonalElement(materials, fillFactors0,
        orientations0).at(0,0,0);
    Precision::RationalFunction xsi1 =
        tgc.diagonalElement(materials, fillFactors1,
        orientations1).at(0,0,0);
    Precision::RationalFunction dXsi = dPermittivity->at(0,0,0);

    if (0)
    {
        cerr << "Mat 1: " << xsi0 << "\nMat 2: " << xsi1 << "\n";
        cerr << "Deriv: " << dXsi << "\n";
        cerr << "DeltNum: " << xsi1.numerator()[0] - xsi0.numerator()[0] << "\n";
        cerr << "DerivNum: " << dXsi.numerator()[0] << "\n";
    }

    BOOST_CHECK(fabs(xsi1.numerator()[0] - xsi0.numerator()[0] -
        dXsi.numerator()[0]) < 0.001);
}


//
// Created by LEI XU on 4/11/19.
//

#ifndef RASTERIZER_TRIANGLE_H
#define RASTERIZER_TRIANGLE_H

#include <Eigen/Core>
#include "Texture.hpp"

using namespace Eigen;
class Triangle{

public:
    Vector4f v[3]; /*the original coordinates of the triangle, v0, v1, v2 in counter clockwise order*/
    /*Per vertex values*/
    Vector3f color[3]; //color at each vertex;
    Vector2f tex_coords[3]; //texture u,v
    Vector3f normal[3]; //normal vector for each vertex

    Texture *tex= nullptr;
    Triangle();

    Eigen::Vector4f a() const { return v[0]; }
    Eigen::Vector4f b() const { return v[1]; }
    Eigen::Vector4f c() const { return v[2]; }

    void setVertex(int ind, Vector4f ver); /*set i-th vertex coordinates */
    void setNormal(int ind, Vector3f n); /*set i-th vertex normal vector*/
    void setColor(int ind, float r, float g, float b); /*set i-th vertex color*/

    void setNormals(const std::array<Vector3f, 3>& normals);
    void setColors(const std::array<Vector3f, 3>& colors);
    void setTexCoord(int ind,Vector2f uv ); /*set i-th vertex texture coordinate*/
    std::array<Vector4f, 3> toVector4() const;
};

#endif //RASTERIZER_TRIANGLE_H


#include <memory>
#include <boost/regex.hpp>

#include "BPMIteration.h"
#include "ParaxialPrimaryEvolutionOperator.h"
#include "PhaseEvolutionOperator.h"
#include "FresnelOperator.h"
#include "SimpleShiftOperator.h"

BPMIteration::BPMIteration(
		const VectorField<double> &lc_field,
		ScreenOpticalFieldCollection &screen_optical_fields,
		const PhysicsCoefficients &coefs,
		const RootSettings &settings) :
	coefs(coefs),
	settings(settings),
	delta_x(lc_field.mesh.delta_x),
	delta_y(lc_field.mesh.delta_y),
	delta_z(lc_field.mesh.delta_z),
	Nx(lc_field.mesh.Nx),
	Ny(lc_field.mesh.Ny),
	Nz(lc_field.mesh.Nz),
	Nz_substeps(settings.algorithm.bpm.Nz_substeps),
	lc_field(lc_field),
	screen_optical_fields(screen_optical_fields),
	bulk_output(settings.postprocessor.volume_output.activate),
	wavelengths(coefs.wavelengths()),
	q_vals(coefs.q_vals()) {

	auto& ic_settings = settings.physics.initial_conditions;
	if(ic_settings.beam_profile_type == "UniformBeam")
		beam_profile_type = BeamProfileType::UniformBeam;
	else {
		boost::smatch match_res;
		if(boost::regex_match(
				ic_settings.beam_profile_type, match_res,
				boost::regex("GaussianBeam\\(([0-9]+\\.?[0-9]*)\\)"))) {
			beam_profile_type = BeamProfileType::GaussianBeam;
			waist = std::stod(match_res[1]);
		}
		else {
			throw std::string("Unrecognised beam profile");
		}
	}

	if(bulk_output)
		bulk_optical_fields = std::make_shared<BulkOpticalFieldCollection>(
			lc_field.mesh, coefs);
}

void BPMIteration::propagate_fields() {

	for(int wave_idx=0; wave_idx<wavelengths.size(); wave_idx++) {
		std::cout <<
			"[ wavelength: " << wavelengths[wave_idx] << "µm ]" << std::endl <<
			"\tInitializing evolution operators..." << std::endl;

		PermittivityTensorField eps(lc_field, coefs, wavelengths[wave_idx]);
		ParaxialPrimaryEvolutionOperator primary_evolution_operator(
			eps, wavelengths[wave_idx], settings);
		PhaseEvolutionOperator secondary_evolution_operator(
			eps, wavelengths[wave_idx], settings);
		FresnelOperator fresnel_operator(eps, coefs.get_nin(wavelengths[wave_idx]));

		std::vector<SimpleShiftOperator> shift_operators;
		for(int q_idx=0; q_idx<q_vals.size(); q_idx++)
			shift_operators.emplace_back(eps, wavelengths[wave_idx], q_vals[q_idx], settings);

		primary_evolution_operator.z_reinit();
		secondary_evolution_operator.z_reinit();

		// We initialize the values of the optical field on the entrance
		// plane
		std::cout <<
			"\tInitializing optical fields..." << std::endl;
		for(int pol_idx=0; pol_idx<2; pol_idx++) {
			for(int q_idx=0; q_idx<q_vals.size(); q_idx++) {
				std::shared_ptr<BeamProfile> beam_profile;
				if(beam_profile_type == BeamProfileType::GaussianBeam)
					beam_profile = std::make_shared<GaussianBeam>(
						coefs, waist, 0.5*PI*pol_idx, wavelengths[wave_idx]);
				else if(beam_profile_type == BeamProfileType::UniformBeam)
					beam_profile = std::make_shared<UniformBeam>(
						coefs, 0.5*PI*pol_idx, wavelengths[wave_idx]);

				double x, y;
				for(int iperp=0; iperp<Nx*Ny; iperp++) {
					x = ((iperp%Nx) - (Nx-1)/2.) * delta_x;
					y = ((iperp/Nx) - (Ny-1)/2.) * delta_y;
					screen_optical_fields(wave_idx,q_idx,pol_idx)(iperp,0) =
						beam_profile->get_Ex(x, y);
					screen_optical_fields(wave_idx,q_idx,pol_idx)(iperp,1) =
						beam_profile->get_Ey(x, y);
				}
				fresnel_operator.apply(
					screen_optical_fields(wave_idx,q_idx,pol_idx));

				if(bulk_output) {
					#pragma omp parallel for
					for(int iperp=0; iperp<Nx*Ny; iperp++)
						for(int comp=0; comp<2; comp++)
							bulk_optical_fields->set_field_val(
								wave_idx, q_idx, pol_idx, comp, iperp,
								screen_optical_fields(wave_idx,q_idx,pol_idx)(iperp,comp));
				}
			}
		}

		// We propagate the fields with the evolution operators.
		std::cout << "\tPropagating optical fields..." << std::endl;
		for(int iz=0; iz<Nz-1; iz++) {
			// We use the evolution operators to compute the optical field in
			// the next transverse plane.
			primary_evolution_operator.update();
			secondary_evolution_operator.update();

			for(int pol_idx=0; pol_idx<2; pol_idx++) {
				for(int q_idx=0; q_idx<q_vals.size(); q_idx++) {
					shift_operators[q_idx].apply(
						screen_optical_fields(wave_idx,q_idx,pol_idx));
					secondary_evolution_operator.apply(
						screen_optical_fields(wave_idx,q_idx,pol_idx));
					primary_evolution_operator.apply(
						screen_optical_fields(wave_idx,q_idx,pol_idx));
					secondary_evolution_operator.apply(
						screen_optical_fields(wave_idx,q_idx,pol_idx));
					shift_operators[q_idx].apply(
						screen_optical_fields(wave_idx,q_idx,pol_idx));
				}
			}

			primary_evolution_operator.z_step_increment();
			secondary_evolution_operator.z_step_increment();

			// We save the calculated optical field values of the new transverse plane in
			// the bulk output object if the user asked for it.
			if(bulk_output) {
				int global_index;
				#pragma omp parallel for firstprivate(global_index)
				for(int iperp=0; iperp<Nx*Ny; iperp++) {
					global_index = (iperp+Nx*Ny*(iz+1));
					for(int pol_idx=0; pol_idx<2; pol_idx++) {
						for(int q_idx=0; q_idx<q_vals.size(); q_idx++) {
							for(int comp=0; comp<2; comp++)
								bulk_optical_fields->set_field_val(
									wave_idx, q_idx, pol_idx, comp, global_index,
									screen_optical_fields(wave_idx,q_idx,pol_idx)(iperp,comp));
						}
					}
				}
			}
		}
	}
}


/*
 * G_mem.cpp
 *
 *  Created on: 08.09.2010
 *      Author: stephaniebayer
 */

#include "G_mem.h"
#include <iostream>

#include <NTL/ZZ.h>
NTL_CLIENT

//Constructos
G_mem::G_mem() {
	// TODO Auto-generated constructor stub

}

G_mem::G_mem(ZZ x){

	val=x;
}

G_mem::G_mem(long x){

	val=to_ZZ(x);
}

ZZ G_mem::value()const{

	return val;
}

//Destructor
G_mem::~G_mem() {
	// TODO Auto-generated destructor stub
}

//Returns the invers of the element
G_mem G_mem::inv(){
	cout << "Returns the invers of the group element" << endl;
	if (val==to_ZZ(0))
		return G_mem(0);
	else
		return G_mem(-1);
}

//General operator, does nothing
G_mem G_mem::op(const G_mem& x, const G_mem& y){
	cout << "Group operation between the element x and y " << endl;
	return G_mem(1);
}

//Equality Check
bool G_mem::operator==(const G_mem& y) const{

	if (val == y.value())
		return true;
	else
		return false;
}


#include <iostream>

#include <cmath>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation.hpp>

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/linear_congruential.hpp>

namespace bnu=boost::numeric::ublas;
//===============FUNCTIONS==================
template<typename T>
std::ostream& operator<< (std::ostream& out, const std::vector<T>& v)
{
   //out << "[";
   size_t last = v.size() - 1;
   for(size_t i = 0; i < v.size(); ++i) {
      out << v[i];
      if (i != last)
         out << " ";
   }
   //out << "]";
   out << std::endl;
   return out;
}
/* Function update H_mew vector with the current number of molecules in each state from the state vector X */
void updateHmew(std::vector<double>& Hmew, const std::vector<int> X)
{
   Hmew[0] = X[0] * X[1];
   Hmew[1] = X[2];
   Hmew[2] = X[2] * X[0];
   Hmew[3] = X[3];
   Hmew[4] = X[2];
   Hmew[5] = X[4];
   Hmew[6] = X[4] * X[0];
   Hmew[7] = X[5];
   Hmew[8] = X[3];
   Hmew[9] = X[5];
   Hmew[10] = X[5];
   Hmew[11] = X[6];
   Hmew[12] = X[3];
   Hmew[13] = X[7];
   Hmew[14] = X[6];
   Hmew[15] = X[7];
}
void updateX(std::vector<int>& X, const int rx_id)
{
   switch(rx_id)
   {
      case 0:
      {
         if((X[0] > 0) && (X[1] > 0)){
            --X[0]; --X[1];
            ++X[2];
         }
         break;
      }
      case 1:
      {
         if(X[2] > 0){
         --X[2];
         ++X[0]; ++X[1];
         }
         break;
      }
      case 2:
      {
         if((X[2] > 0) && (X[0] > 0)){
         --X[2]; --X[0];
         ++X[3];
         }
         break;
      }
      case 3:
      {
         if(X[3] > 0){
         --X[3];
         ++X[2]; ++X[0];
         }
         break;
      }
      case 4:
      {
         if(X[2] > 0){
         --X[2];
         ++X[4];
         }
         break;
      }
      case 5:
      {
         if(X[4] > 0){
         --X[4];
         ++X[2];
         }
         break;
      }
      case 6:
      {
         if((X[4] > 0) && (X[0] > 0)){
         --X[4];
         --X[0];
         ++X[5];
         }
         break;
      }
      case 7:
      {
         if(X[5] > 0){
         --X[5];
         ++X[4];
         ++X[0];
         }
         break;
      }
      case 8:
      {
         if(X[3] > 0){
         --X[3];
         ++X[5];
         }
         break;
      }
      case 9:
      {
         if(X[5] > 0){
         --X[5];
         ++X[3];
         }
         break;
      }
      case 10:
      {
         if(X[5] > 0){
         --X[5];
         ++X[6];
         }
         break;
      }
      case 11:
      {
         if(X[6] > 0){
         --X[6];
         ++X[5];
         }
         break;
      }
      case 12:
      {
         if(X[3] > 0){
         --X[3];
         ++X[7];
         }
         break;
      }
      case 13:
      {
         if(X[7] > 0){
         --X[7];
         ++X[3];
         }
         break;
      }
      case 14:
      {
         if(X[6] > 0){
         --X[6];
         ++X[7];
         }
         break;
      }
      case 15:
      {
         if(X[7] > 0){
         --X[7];
         ++X[6];
         }
         break;
      }
   }
   for(size_t i=0; i<X.size();++i){
      if(X[i] < 0){
         X[i] = 0;
      }
   }
}
template <typename vec_type, typename mat_type>
void MatrixCopy(const std::vector<vec_type> v, bnu::matrix<mat_type> &m, const bool byrow)
/*Copy a vector v to the matrix m in either row or columnwise*/
{
   if(v.size() != m.size1()*m.size2()){
      std::cout << "rows of the matrix = " << m.size1() << std::endl << "columns of matrix = " << m.size2() << std::endl;
      std::cout << "size of vector = " << v.size() << std::endl;
      std::cout << "size of vector not equal to sum of row and columns of the given matrix" << std::endl;
      return;
   }
      if(byrow){
         for(size_t row=0; row<m.size1(); row++){
            for(size_t col=0; col<m.size2(); col++){
               m(row,col) = v[(row*(m.size2()))+(col)];
            }
         }
      }
      else{
         for(size_t col=0; col<m.size2(); col++){
            for(size_t row=0; row<m.size1(); row++){
               m(row,col) = v[(col*(m.size1()))+(row)];
            }
         }
      }

}
//----------
template< typename T >
T vectorSum(const std::vector<T> v)
{
   T sum=0;
   for(size_t i=0; i < v.size();i++){
      sum = sum + v[i];
   }
   return sum;
}
//----------
std::vector<double> vectorProd(const std::vector<double> a, const std::vector<double> b)
{
   std::vector<double> c( a.size() );
   if(a.size() == b.size()){
      for(size_t i=0; i < a.size();i++){
         c[i] = (a[i] * b[i]);
      }
   }
   return(c);
}
//----------
template< typename T >
std::vector<T> vectorCumSum(const std::vector<T> v)
{
   std::vector<T> cumsum;
   T sum = 0;
   for(size_t i=0; i < v.size();i++){
      sum = sum + v[i];
      cumsum.push_back(sum);
   }
   return(cumsum);
}
std::vector<double> colSum(const bnu::matrix<double> &m)
{
   std::vector<double> colsum;
   for(size_t col=0; col < m.size2(); col++){
      double sum=0.0;
      for(size_t row=0; row < m.size1(); row++){
         sum = sum + m(row,col);
      }
      colsum.push_back(sum);
   }
   return colsum;
}
//------------------
std::vector<double> rowSum(const bnu::matrix<double> &m)
{
   std::vector<double> rowsum;
   for(size_t row=0; row < m.size1(); row++){
      double sum=0.0;
      for(size_t col=0; col < m.size2(); col++){
         sum = sum + m(row,col);
      }
      rowsum.push_back(sum);
   }
   return rowsum;
}
//Function to compute Tau from the first random number r1 and aTot (a = ∑H_mew*C_mew)
double getTau(double r1, double aTot)
{
   if (aTot >1E-308){
      return( (1/aTot) * log(1/r1) );
   }
   else
      return(-1);
}
//Function to compute mew_index from the second random number r1 and a_mew (the vector h_new*c_new; new: 0 to M reactions)
int getMewIndex(double r2, double aTot, std::vector<double> amew0)
{
   std::vector<int> amew;
   for(size_t i=0; i<amew0.size();i++){
      if(amew0[i] > 1E-308){
         amew.push_back(i);
      }
   }
   int j=-1;
   if (!amew.empty()){
      j = amew[0];
      double cum=0;
      for(size_t i=0; i<amew.size()-1;i++){
         cum = cum + amew0[amew[i]];
         if (cum <= (aTot * r2) ){
            j = amew[i+1];
         }
      }
   }
   //std::cout << amew << std::endl;
   return(j);
}
//------------------------
//This is typedef for a random number generator
typedef boost::minstd_rand base_generator_type;
//-------------------------
//=================MAIN====================
int main(int argc, char** argv)
{
   const double avagadro = 6.022e+23;
   //--------------
   double kf1 = 4.59E06;//M^-1 S^-1
   double kb1 = 4.26E03;//S^-1
   double kf2 = 28.4E06;//M^-1 * S^-1
   double kb2 = 3.26E03;//S^-1
   double kf3 = 1.27E06;//M^-1 S^-1
   double kb3 = 45.7;//S^-1
   double a0 = 4.24E03;//S^-1
   double b0 = 900.0;//S^-1
   double a1 = 2.89E03;//S^-1
   double b1 = 39.2;//S^-1
   double a2 = 172.0;//S^-1
   double b2 = 0.727;//S^-1
   double a3 = 17.7;//S^-1
   double b3 = 4.0;//S^-1
   double a4 = 16.8;//S^-1
   double b4 = 190.4;//S^-1
   //-----------------------------
   //Define a random number generator and initialize it with a reproducible seed
   base_generator_type generator(45);
   //Define a uniform random number distribution which produces "double"
   //values b/w 0 and 1 (0 inclusive, 1 exlusive).
   boost::uniform_real<>uni_dist(0,1);
   boost::variate_generator<base_generator_type&, boost::uniform_real<> > uni(generator, uni_dist);
   std::vector<int> X(8);// A vector to hold the initial number of the receptors in each states [Glu,C0,C1,C2,C3,C4,C5,Open]
   X = {500000,100,0,0,0,0,0,0};
   std::vector<double> Cmew(16); //A vector to hold the Cmew values
   double V = 1.0;// Volume of the reaction space
   Cmew = {kf1*V,kb1,kf2*V,kb2,a1,b1,kf3*V,kb3,a2,b2,a4,b4,a0,b0,b3,a3};
   std::vector<double> Hmew(16); //A vector to hold the Hmew values
   std::vector<double> Amew(16); //A vector to hold the Amew values
   std::transform(Cmew.begin(),Cmew.end(),Cmew.begin(),std::bind1st(std::multiplies<double>(),(1) ));
   Hmew = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
   double tmax = 10.0;//10 secs
   double dt = 50E-06;// 1 msec
   double t2 = dt;
   double t = 0;
   double r1 = 0.0;
   double r2 = 0.0;
   int mewIndex = 0;
   double tau = 0.0;
   double AmewTot = 0.0;
   int RxIndex = 0;
   std::cout << t << " " << X;
   while(t <= 1){
      updateHmew(Hmew,X); // calculate Hmew based on the current state
      Amew = vectorProd(Hmew,Cmew);// calculate Amew = Hmew*Cmew
      AmewTot = vectorSum(Amew);// store Amew total
      //Genrate two random numbers from the distribution by means of STL generator interface.
      r1 = uni();
      r2 = uni();
      tau = getTau(r1,AmewTot); //calutate the next time step dttau
      if ( (mewIndex != -1) ){
         mewIndex = getMewIndex(r2,AmewTot,Amew); // determine the next reaction
         updateX(X,mewIndex); // update the state to reflect the occurance of the reaction identified by mewIndex
      }
      t = t+tau;
      if (t >= t2){
         std::cout << t << " " << X;
         t2+=dt;
      }
      RxIndex+=1;
      //std::cout << "Step = " << RxIndex << "   Time = " << t <<  "  Rx time step = " << tau << "   Rx index  = " << mewIndex << std::endl;
      //std::cout << "Cmew = " << Cmew;
      //std::cout << "Hmew = " << Hmew;
      //std::cout << "Amew Tot = " << AmewTot << "   r2 = " << r2  << "   Amew * r2 = " << AmewTot*r2  << " " << "   Amew = "  << Amew;
      //std::cout << vectorCumSum(Amew);
      //std::cin.get();
   }
}


// Copyright 2014 Velodyne Acoustics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "vtkLASFileWriter.h"

#include <vtkPointData.h>
#include <vtkPolyData.h>

#include <vtk_libproj.h>

#include <liblas/liblas.hpp>

#include <Eigen/Dense>

#ifndef PJ_VERSION // 4.8 or later
#include <cassert>
#endif

namespace
{

#ifdef PJ_VERSION // 4.8 or later

//-----------------------------------------------------------------------------
projPJ CreateProj(int epsg)
{
  std::ostringstream ss;
  ss << "+init=epsg:" << epsg << " ";
  return pj_init_plus(ss.str().c_str());
}

//-----------------------------------------------------------------------------
Eigen::Vector3d ConvertGcs(Eigen::Vector3d p, projPJ inProj, projPJ outProj)
{
  if (pj_is_latlong(inProj))
  {
    p[0] *= DEG_TO_RAD;
    p[1] *= DEG_TO_RAD;
  }

  double* const data = p.data();
  //std::cout << "position in : [" << p[0] << ";" << p[1] << ";" << p[2] << "]" << std::endl;
  int last_errno = pj_transform(inProj, outProj, 1, 1, data + 0, data + 1, data + 2);
  //std::cout << "position out : [" << p[0] << ";" << p[1] << ";" << p[2] << "]" << std::endl << std::endl;

  if (last_errno != 0)
  {
    vtkGenericWarningMacro("Error : CRS conversion failed with error: " << last_errno);
  }

  if (pj_is_latlong(outProj))
  {
    p[0] *= RAD_TO_DEG;
    p[1] *= RAD_TO_DEG;
  }

  return p;
}

#else

//-----------------------------------------------------------------------------
PROJ* CreateProj(int utmZone, bool south)
{
  std::ostringstream ss;
  ss << "+zone=" << utmZone;

  char buffer[65] = { 0 };
  strncpy(buffer, ss.str().c_str(), 64);

  std::vector<const char*> utmparams;
  utmparams.push_back("+proj=utm");
  utmparams.push_back("+ellps=WGS84");
  utmparams.push_back("+units=m");
  utmparams.push_back("+no_defs");
  utmparams.push_back(buffer);
  if (south)
  {
    utmparams.push_back("+south");
  }

  return proj_init(utmparams.size(), const_cast<char**>(&(utmparams[0])));
}

//-----------------------------------------------------------------------------
Eigen::Vector3d InvertProj(Eigen::Vector3d in, PROJ* proj)
{
  // This "lovely little gem" makes an awful lot of assumptions about the input
  // (some flavor of XY) and output (internal LP, which we assume / hope is
  // WGS'84) GCS's and what operations are "interesting" (i.e. the assumption
  // that the Z component does not need to be considered and can be passed
  // through unaltered). Unfortunately, it's the best we can do with PROJ 4.7
  // until VTK can be updated to use 4.8. Fortunately, given how we're being
  // used, our input really ought to always be UTM.
  PROJ_XY xy;
  xy.x = in[0];
  xy.y = in[1];

  const PROJ_LP lp = proj_inv(xy, proj);
  return Eigen::Vector3d(lp.lam * RAD_TO_DEG, lp.phi * RAD_TO_DEG, in[2]);
}

#endif
}

//-----------------------------------------------------------------------------
class vtkLASFileWriter::vtkInternal
{
public:
  vtkInternal()
  {
    this->IsWriterInstanciated = false;
  }

  void Close();

  std::ofstream Stream;
  liblas::Writer* Writer;

  double MinTime;
  double MaxTime;
  Eigen::Vector3d Origin;

  size_t npoints;
  double MinPt[3];
  double MaxPt[3];

  liblas::Header header;
  bool IsWriterInstanciated;

#ifdef PJ_VERSION // 4.8 or later
  projPJ InProj;
  projPJ OutProj;
#else
  PROJ* Proj;
#endif
  int OutGcs;
};

//-----------------------------------------------------------------------------
void vtkLASFileWriter::vtkInternal::Close()
{
  delete this->Writer;
  this->Writer = 0;
  this->Stream.close();
}

//-----------------------------------------------------------------------------
vtkLASFileWriter::vtkLASFileWriter(const char* filename)
  : Internal(new vtkInternal)
{
  this->Internal->MinTime = -std::numeric_limits<double>::infinity();
  this->Internal->MaxTime = +std::numeric_limits<double>::infinity();

#ifdef PJ_VERSION // 4.8 or later
  this->Internal->InProj = 0;
  this->Internal->OutProj = 0;
#else
  this->Internal->Proj = 0;
#endif
  this->Internal->OutGcs = -1;

  this->Internal->npoints = 0;

  for (int i = 0; i < 3; ++i)
  {
    this->Internal->MaxPt[i] = -std::numeric_limits<double>::max();
    this->Internal->MinPt[i] = std::numeric_limits<double>::max();
  }

  this->Internal->Stream.open(filename, std::ios::out | std::ios::trunc | std::ios::binary);

  this->Internal->header.SetSoftwareId(SOFTWARE_NAME);
  this->Internal->header.SetDataFormatId(liblas::ePointFormat1);
  this->Internal->header.SetScale(1e-3, 1e-3, 1e-3);
}

//-----------------------------------------------------------------------------
vtkLASFileWriter::~vtkLASFileWriter()
{
  this->Internal->Close();

#ifdef PJ_VERSION // 4.8 or later
  pj_free(this->Internal->InProj);
  pj_free(this->Internal->OutProj);
#else
  proj_free(this->Internal->Proj);
#endif

  delete this->Internal;
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::SetTimeRange(double min, double max)
{
  this->Internal->MinTime = min;
  this->Internal->MaxTime = max;
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::SetOrigin(int gcs, double easting, double northing, double height)
{
  if (this->Internal->IsWriterInstanciated)
  {
    vtkGenericWarningMacro("Header can't be changed once the writer is instanciated");
    return;
  }

  Eigen::Vector3d origin(northing, easting, height);
  this->Internal->Origin = origin;

  // Convert offset to output GCS, if a geoconversion is set up
#ifdef PJ_VERSION // 4.8 or later
  if (this->Internal->OutProj)
  {
    origin = ConvertGcs(origin, this->Internal->InProj, this->Internal->OutProj);
    gcs = this->Internal->OutGcs;
  }
#else
  if (this->Internal->Proj)
  {
    origin = InvertProj(origin, this->Internal->Proj);
    gcs = this->Internal->OutGcs;
  }
#endif

  // Update header
  this->Internal->header.SetOffset(origin[0], origin[1], origin[2]);
  try
  {
    liblas::SpatialReference srs;
    std::ostringstream ss;
    ss << "EPSG:" << gcs;
    srs.SetFromUserInput(ss.str());
    std::cout << srs << std::endl;
    this->Internal->header.SetSRS(srs);
  }
  catch (std::logic_error)
  {
    std::cerr << "failed to set SRS (logic)" << std::endl;
  }
  catch (std::runtime_error)
  {
    std::cerr << "failed to set SRS" << std::endl;
  }
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::SetGeoConversion(int in, int out)
{
#ifdef PJ_VERSION // 4.8 or later
  pj_free(this->Internal->InProj);
  pj_free(this->Internal->OutProj);

  this->Internal->InProj = CreateProj(in);
  this->Internal->OutProj = CreateProj(out);
#else
  // The PROJ 4.7 API makes it near impossible to do generic transforms, hence
  // InvertProj (see also comments there) is full of assumptions. Assert some
  // of those assumptions here.
  assert((in > 32600 && in < 32661) || (in > 32700 && in < 32761));
  assert(out == 4326);

  proj_free(this->Internal->Proj);
  this->Internal->Proj = CreateProj(in % 100, in > 32700);
#endif

  this->Internal->OutGcs = out;
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::SetGeoConversion(int in, int out, int utmZone, bool isLatLon)
{
#ifdef PJ_VERSION // 4.8 or later
  in = in;  // this was just added to avoid the warning: "parameter 'in' is not used"

  std::stringstream utmparamsIn;
  utmparamsIn << "+proj=utm ";
  std::stringstream zone;
  zone << "+zone=" << utmZone;
  std::string UTMString = zone.str();
  utmparamsIn << UTMString << " ";
  utmparamsIn << "+datum=WGS84 ";
  utmparamsIn << "+units=m ";
  utmparamsIn << "+no_defs ";
  this->Internal->InProj = pj_init_plus(utmparamsIn.str().c_str());
  std::cout << "init In : " << utmparamsIn.str() << std::endl;

  if (isLatLon)
  {
    std::stringstream utmparamsOut;
    utmparamsOut << "+proj=longlat ";
    utmparamsOut << "+ellps=WGS84 ";
    utmparamsOut << "+datum=WGS84 ";
    utmparamsOut << "+no_defs ";
    this->Internal->OutProj = pj_init_plus(utmparamsOut.str().c_str());
    std::cout << "init Out : " << utmparamsOut.str() << std::endl;
  }
  else
  {
    std::stringstream utmparamsOut;
    utmparamsOut << "+proj=utm ";
    std::stringstream zone;
    zone << "+zone=" << utmZone;
    std::string UTMString = zone.str();
    utmparamsOut << UTMString << " ";
    utmparamsOut << "+ellps=WGS84 ";
    utmparamsOut << "+datum=WGS84 ";
    utmparamsOut << "+no_defs ";
    this->Internal->OutProj = pj_init_plus(utmparamsOut.str().c_str());
  }

  std::cout << "InProj :  created : " << this->Internal->InProj << std::endl;
  std::cout << "OutProj created : " << this->Internal->OutProj << std::endl;
  if (this->Internal->InProj)
    std::cout << "inProj datum_type : [" << this->Internal->InProj->datum_type << "]" << std::endl;
  if (this->Internal->OutProj)
    std::cout << "outProj datum_type : [" << this->Internal->OutProj->datum_type << "]" << std::endl;
#else
  // The PROJ 4.7 API makes it near impossible to do generic transforms, hence
  // InvertProj (see also comments there) is full of assumptions. Assert some
  // of those assumptions here.
  assert((in > 32600 && in < 32661) || (in > 32700 && in < 32761));
  assert(out == 4326);

  proj_free(this->Internal->Proj);
  this->Internal->Proj = CreateProj(in % 100, in > 32700);
#endif

  this->Internal->OutGcs = out;
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::SetPrecision(double neTol, double hTol)
{
  if (this->Internal->IsWriterInstanciated)
  {
    vtkGenericWarningMacro("Header can't be changed once writer is instanciated");
    return;
  }

  this->Internal->header.SetScale(neTol, neTol, hTol);
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::WriteFrame(vtkPolyData* data)
{
  if (!this->Internal->IsWriterInstanciated)
  {
    this->Internal->Writer = new liblas::Writer(this->Internal->Stream, this->Internal->header);
    this->Internal->IsWriterInstanciated = true;
  }

  vtkPoints* const points = data->GetPoints();
  vtkDataArray* const intensityData = data->GetPointData()->GetArray("intensity");
  vtkDataArray* const laserIdData = data->GetPointData()->GetArray("laser_id");
  vtkDataArray* const timestampData = data->GetPointData()->GetArray("timestamp");

  const vtkIdType numPoints = points->GetNumberOfPoints();
  for (vtkIdType n = 0; n < numPoints; ++n)
  {
    const double time = timestampData->GetComponent(n, 0) * 1e-6;
    if (time >= this->Internal->MinTime && time <= this->Internal->MaxTime)
    {
      Eigen::Vector3d pos;
      points->GetPoint(n, pos.data());
      pos += this->Internal->Origin;

#ifdef PJ_VERSION // 4.8 or later
      if (this->Internal->OutProj)
      {
        pos = ConvertGcs(pos, this->Internal->InProj, this->Internal->OutProj);
      }
#else
      if (this->Internal->Proj)
      {
        pos = InvertProj(pos, this->Internal->Proj);
      }
#endif

      liblas::Point p(&this->Internal->Writer->GetHeader());
      p.SetCoordinates(pos[0], pos[1], pos[2]);
      p.SetIntensity(static_cast<uint16_t>(intensityData->GetComponent(n, 0)));
      p.SetReturnNumber(1);
      p.SetNumberOfReturns(1);
      p.SetUserData(static_cast<uint8_t>(laserIdData->GetComponent(n, 0)));
      p.SetTime(time);

      this->Internal->Writer->WritePoint(p);
    }
  }
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::FlushMetaData()
{
  this->Internal->header.SetPointRecordsByReturnCount(0, this->Internal->npoints);
  this->Internal->header.SetMin(this->Internal->MinPt[0], this->Internal->MinPt[1], this->Internal->MinPt[2]);
  this->Internal->header.SetMax(this->Internal->MaxPt[0], this->Internal->MaxPt[1], this->Internal->MaxPt[2]);
}

//-----------------------------------------------------------------------------
void vtkLASFileWriter::UpdateMetaData(vtkPolyData* data)
{
  vtkPoints* const points = data->GetPoints();
  vtkDataArray* const timestampData = data->GetPointData()->GetArray("timestamp");

  const vtkIdType numPoints = points->GetNumberOfPoints();
  for (vtkIdType n = 0; n < numPoints; ++n)
  {
    const double time = timestampData->GetComponent(n, 0) * 1e-6;
    if (time >= this->Internal->MinTime && time <= this->Internal->MaxTime)
    {
      Eigen::Vector3d pos;
      points->GetPoint(n, pos.data());
      pos += this->Internal->Origin;

#ifdef PJ_VERSION // 4.8 or later
      if (this->Internal->OutProj)
      {
        pos = ConvertGcs(pos, this->Internal->InProj, this->Internal->OutProj);
      }
#else
      if (this->Internal->Proj)
      {
        pos = InvertProj(pos, this->Internal->Proj);
      }
#endif

      this->Internal->npoints++;

      for (int i = 0; i < 3; ++i)
      {
        if (pos[i] > this->Internal->MaxPt[i])
        {
          this->Internal->MaxPt[i] = pos[i];
        }
        if (pos[i] < this->Internal->MinPt[i])
        {
          this->Internal->MinPt[i] = pos[i];
        }
      }
    }
  }
}