#include <iostream>
#include <sstream>
#include <assert.h>
#include <math.h>
#include <cmath>
#include <iomanip>
#include "../flt.hpp"
#include <boost/algorithm/string.hpp>

using namespace epilog::pow;

static void header( const std::string &str )
{
    std::cout << "\n";
    std::cout << "--- [" + str + "] " + std::string(60 - str.length(), '-') << "\n";
    std::cout << "\n";
}

static bool tol_check(double d, flt1648 v, double tol=0.00001)
{
    double vd = v.to_double();
    assert((d < 0) == (vd < 0));
    if (vd == d) {
	return true;
    }
    double rel = fabs(0.5 - fabs(vd) / (fabs(d)+fabs(vd)));
    assert(rel < tol);
    return true;
}

const size_t N = 10;
const double db[N] = { 2.3e-7, 2.134e+10, -5.91352e+5, 2.12345e-3, 2e+256,
		       123.456e-127, 42e-42, -99e+42, -5e-17, -4.321e+19 };
flt1648 fl[N];

class next_count {
public:
    friend std::ostream & operator << (std::ostream &out, const next_count &nc);
};

static int cnt = 0;

inline std::ostream & operator << (std::ostream &out, const next_count &nc) {
    cnt++;
    out << "[" << cnt << "]: ";
    return out;
}

static void test_flt_init()
{
    std::cout.precision(17);

    for (size_t i = 0; i < N; i++) {
	int exp = 0;
	double frac = frexp(db[i], &exp);
	fl[i] = flt1648( exp, fxp1648(frac));
	std::cout << next_count() << "db[" << i << "]=" << db[i] << std::endl;
	std::cout << next_count() << "fl[" << i << "]=" << fl[i] << std::endl;
	tol_check(db[i], fl[i]);
    }
}

static void test_flt_add()
{
    header("test_flt_add");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "+" << bd << "=" << (ad+bd) << std::endl;
	    std::cout << next_count() << af << "+" << bf << "=" << (af+bf) << std::endl;
	    tol_check((ad+bd), (af+bf));
	}
    }
}

static void test_flt_sub()
{
    header("test_flt_sub");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "-" << bd << "=" << (ad-bd) << std::endl;
	    std::cout << next_count() << af << "-" << bf << "=" << (af-bf) << std::endl;
	    tol_check((ad-bd), (af-bf));
	}
    }
}

static void test_flt_mul()
{
    header("test_flt_mul");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "*" << bd << "=" << (ad*bd) << std::endl;
	    std::cout << next_count() << af << "*" << bf << "=" << (af*bf) << std::endl;
	    tol_check((ad*bd), (af*bf));
	}
    }
}

static void test_flt_div()
{
    header("test_flt_div");

    for (size_t i = 0; i < N; i++) {
	for (size_t j = 0; j < N; j++) {
	    double ad = db[i], bd = db[j];
	    flt1648 af = fl[i], bf = fl[j];
	    std::cout << next_count() << ad << "/" << bd << "=" << (ad/bd) << std::endl;
	    std::cout << next_count() << af << "/" << bf << "=" << (af/bf) << std::endl;
	    tol_check((ad/bd), (af/bf));
	}
    }
}

static void test_flt_reciprocal()
{
    header("test_flt_reciprocal");

    for (size_t i = 0; i < N; i++) {
	double ad = db[i];
	flt1648 af = fl[i];
	double ad_reciprocal = 1.0 / ad;
	flt1648 af_reciprocal = af.reciprocal();
	std::cout << next_count() << ad << " reciprocal=" << ad_reciprocal << std::endl;
	std::cout << next_count() << af << " reciprocal=" << af_reciprocal << std::endl;
	tol_check(ad_reciprocal, af_reciprocal);
    }
}

static void test_flt_values()
{
    header("test_flt_values");

    auto v = flt1648::from(12, 1234567);

    std::cout << "VALUE: " << v.to_double() << std::endl;

    tol_check(12.1234567, v);
}

static void test_bitcoin_difficulty()
{
    header("test_bitcoin_difficulty");

    auto base_value = flt1648(0x0404cb);
    auto mult_value = flt1648(1) << (8*(0x1b - 3));
    auto target_value = base_value * mult_value;
    std::string expect = "0x00000000000404CB000000000000000000000000000000000000000000000000";
    auto target = "0x" + boost::to_upper_copy(target_value.to_integer_string(32));
    std::cout << "TARGET: " << target << std::endl;
    std::cout << "EXPECT: " << expect << std::endl;
    assert( expect == target );

    auto max_value = flt1648(0x00ffff) * (flt1648(1) << (8*(0x1d - 3)));
    auto difficulty = max_value / target_value;
    std::cout << "Difficulty: " << difficulty.to_double() << std::endl;
    std::cout << "Expect    : 16307.42..." << std::endl;

    assert(abs(difficulty.to_double() - 16307.42) < 0.01);
}

static void test_our_difficulty()
{
    header("test_out_difficulty");

    auto base_value = flt1648(16307) << 32;
    auto target_value = flt1648(1) / base_value;
    auto difficulty = flt1648(1) - target_value;

    std::cout << "DIFFICULTY: " << difficulty.to_double() << std::endl;

    auto rel_target = flt1648(1) - difficulty;

    std::cout << "REL TARGET: " << rel_target.to_double() << std::endl;

    auto max_target = flt1648(1) << 256;
    auto target = rel_target * max_target;

    std::cout << "TARGET    : " << target.to_double() << std::endl;

    std::cout << "TARGET INT: 0x" << target.to_integer_string(32) << std::endl;
}

int main(int argc, char *argv[])
{
    test_flt_init();
    test_flt_add();
    test_flt_sub();
    test_flt_mul();
    test_flt_div();
    test_flt_reciprocal();
    test_flt_values();
    test_bitcoin_difficulty();
    test_our_difficulty();

    return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Array3d v(M_PI, M_PI/2, M_PI/3);
cout << v.cos() << endl;

  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/utility.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/operation/svd.hpp>

using namespace std;
int main(int, char**)
{
    using namespace mtl;
    unsigned size=3, row= size, col=size+1;

    double normA(0), tol(0.0000001);
    dense_vector<double>                    vec(size), vec1(size);
    dense2D<double>                         A(row, col), A_t(row, col), S(row, row), V(row, col), D(col,col), norm(row, col),
					    AT(col,row), A_tT(col,row), ST(col, col), VT(col,row), DT(row,row), normT(col, row);
    A= 0;

    A[0][0]=1;    A[0][1]=1;    A[0][2]=1;
    A[1][0]=1;    A[1][1]=2;    A[1][2]=2;
    A[2][0]=9;    A[2][1]=3;    A[2][2]=2;
    A[2][3]=4;    A[0][3]=4;    A[1][3]=3;
    std::cout<<"A=\n"<< A <<"\n";
    AT= trans(A);
    std::cout<<"START--------------\n";

    boost::tie(S, V, D)= svd(A, tol);
    std::cout<<"MAtrix  S=\n"<< S <<"\n";
    std::cout<<"MAtrix  V=\n"<< V <<"\n";
    std::cout<<"MAtrix  D=\n"<< D <<"\n";
    A_t= S*V*trans(D);
    std::cout<<"MAtrix  A=S*V*D'=\n"<< A_t <<"\n";
    std::cout<<"Original A==\n"<< A <<"\n";
    norm= A_t - A;
    normA= one_norm(norm);
    std::cout<< "norm(SVD-A)=" << normA << "\n";
//     if (normA > size*size*tol) throw mtl::logic_error("wrong SVD decomposition of matrix A");
    std::cout<<"START--------------\n";
#if 1
    boost::tie(ST, VT, DT)= svd(AT, tol);
    std::cout<<"MAtrix  ST=\n"<< ST <<"\n";
    std::cout<<"MAtrix  VT=\n"<< VT <<"\n";
    std::cout<<"MAtrix  DT=\n"<< DT <<"\n";
    A_tT= ST*VT*trans(DT);
    std::cout<<"MAtrix  AT=S*V*D'=\n"<< A_tT <<"\n";
    std::cout<<"Original A==\n"<< AT <<"\n";
    normT= A_tT - AT;
    normA= one_norm(normT);
    std::cout<< "norm(SVD-A)=" << normA << "\n";
#endif
    return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define _USE_MATH_DEFINES
#include <algorithm>
#include <iostream>
#include <vector>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>

#include "perf.hpp"

namespace compute = boost::compute;

using compute::float2_;

float rand_float()
{
    return (float(rand()) / float(RAND_MAX)) * 1000.f;
}

void serial_cartesian_to_polar(const float *input, size_t n, float *output)
{
    for(size_t i = 0; i < n; i++){
        float x = input[i*2+0];
        float y = input[i*2+1];

        float magnitude = std::sqrt(x*x + y*y);
        float angle = std::atan2(y, x) * 180.f / M_PI;

        output[i*2+0] = magnitude;
        output[i*2+1] = angle;
    }
}

void serial_polar_to_cartesian(const float *input, size_t n, float *output)
{
    for(size_t i = 0; i < n; i++){
        float magnitude = input[i*2+0];
        float angle = input[i*2+1];

        float x = magnitude * cos(angle);
        float y = magnitude * sin(angle);

        output[i*2+0] = x;
        output[i*2+1] = y;
    }
}

// converts from cartesian coordinates (x, y) to polar coordinates (magnitude, angle)
BOOST_COMPUTE_FUNCTION(float2_, cartesian_to_polar, (float2_ p),
{
    float x = p.x;
    float y = p.y;

    float magnitude = sqrt(x*x + y*y);
    float angle = atan2(y, x) * 180.f / M_PI;

    return (float2)(magnitude, angle);
});

// converts from polar coordinates (magnitude, angle) to cartesian coordinates (x, y)
BOOST_COMPUTE_FUNCTION(float2_, polar_to_cartesian, (float2_ p),
{
    float magnitude = p.x;
    float angle = p.y;

    float x = magnitude * cos(angle);
    float y = magnitude * sin(angle);

    return (float2)(x, y)
});

int main(int argc, char *argv[])
{
    perf_parse_args(argc, argv);

    std::cout << "size: " << PERF_N << std::endl;

    // setup context and queue for the default device
    compute::device device = compute::system::default_device();
    compute::context context(device);
    compute::command_queue queue(context, device);
    std::cout << "device: " << device.name() << std::endl;

    // create vector of random numbers on the host
    std::vector<float> host_vector(PERF_N*2);
    std::generate(host_vector.begin(), host_vector.end(), rand_float);

    // create vector on the device and copy the data
    compute::vector<float2_> device_vector(PERF_N, context);
    compute::copy_n(
        reinterpret_cast<float2_ *>(&host_vector[0]),
        PERF_N,
        device_vector.begin(),
        queue
    );

    perf_timer t;
    for(size_t trial = 0; trial < PERF_TRIALS; trial++){
        t.start();
        compute::transform(
            device_vector.begin(),
            device_vector.end(),
            device_vector.begin(),
            cartesian_to_polar,
            queue
        );
        queue.finish();
        t.stop();
    }
    std::cout << "time: " << t.min_time() / 1e6 << " ms" << std::endl;

    // perform saxpy on host
    t.clear();
    for(size_t trial = 0; trial < PERF_TRIALS; trial++){
        t.start();
        serial_cartesian_to_polar(&host_vector[0], PERF_N, &host_vector[0]);
        t.stop();
    }
    std::cout << "host time: " << t.min_time() / 1e6 << " ms" << std::endl;

    std::vector<float> device_data(PERF_N*2);
    compute::copy(
        device_vector.begin(),
        device_vector.end(),
        reinterpret_cast<float2_ *>(&device_data[0]),
        queue
    );

    for(size_t i = 0; i < PERF_N; i++){
        float host_value = host_vector[i];
        float device_value = device_data[i];

        if(std::abs(device_value - host_value) > 1e-3){
            std::cout << "ERROR: "
                      << "value at " << i << " "
                      << "device_value (" << device_value << ") "
                      << "!= "
                      << "host_value (" << host_value << ")"
                      << std::endl;
            return -1;
        }
    }

    return 0;
}


#include <map>
#include <string>
#include <vector>
#include <iostream>
#include <Eigen/Dense>
#include "../include/two_layer_net.h"
#include "../datasets/include/mnist.h"
#include "../matplotlibcpp.h"

using namespace Eigen;
namespace plt = matplotlibcpp;

int main()
{
    using std::cout;
    using std::endl;
    using std::map;
    using std::vector;
    using std::string;
    using namespace MyDL;

    // ハイパーパラメータ
    int num_iters = 3000;
    double learning_rate = 0.05;
    int batch_size = 100;
    int input_size = 28 * 28;
    int hidden_size = 100;
    int output_size = 10;

    // MNISTデータローダ
    MnistEigenDataset mnist(batch_size);

    // 各種変数初期化
    MatrixXd train_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd train_y = MatrixXd::Zero(batch_size, output_size);
    MatrixXd test_X = MatrixXd::Zero(batch_size, input_size);
    MatrixXd test_y = MatrixXd::Zero(batch_size, output_size);
    bool one_hot_label = true;

    // ネットワーク生成
    TwoLayerNet net(input_size, hidden_size, output_size, 0.01);

    // 最適化用
    map<string, MatrixXd> grads;
    double loss;
    double accuracy;

    // 学習経過プロット用
    vector<double> loss_history(num_iters);
    vector<int> plot_counter(num_iters);
    vector<double> accuracy_history(num_iters / 10);
    vector<int> accuracy_counter(num_iters / 10);

    // 最適化実行
    for (int i = 0; i < num_iters; i++){
        // 次のミニバッチ取得
        mnist.next_train(train_X, train_y, one_hot_label);

        // 勾配計算
        grads = net.gradient(train_X, train_y); // 内部で

        // 勾配更新
        for (auto i = grads.begin(); i != grads.end(); i++){
            net.params[i->first] -= learning_rate * grads[i->first];
        }

        // 損失プロット用
        loss = net.loss(train_X, train_y);
        loss_history[i] = loss;
        plot_counter[i] = i;

        cout << "iteration" << i << " loss: " << loss << endl;

        // 10step毎にaccuracy計測
        if (i % 10 == 0){
            mnist.next_test(test_X, test_y, one_hot_label);
            accuracy = net.accuracy(test_X, test_y);

            cout << "accuracy: " << accuracy << endl;

            accuracy_history[i/10] = accuracy;
            accuracy_counter[i/10] = i / 10;
        }
    }

    // visualize
    plt::title("Loss History");
    plt::plot(plot_counter, loss_history, "b");
    plt::grid(true);
    plt::save("mnist_learning_curve.png");

    plt::cla();
    plt::title("Accuracy History");
    plt::plot(accuracy_counter, accuracy_history, "r");
    plt::grid(true);
    plt::save("mnist_accuracy_plot.png");

    return 0;
}


#include <iterator>
#include <string>
#include <ctime>
#include <vector>
#include <iostream>
#include <fstream>
#include <boost/assign/std/vector.hpp>
#include <boost/random.hpp>
#include <boost/assert.hpp>
#include <boost/numeric/ublas/io.hpp>
#include<iostream>
#include "expm.hpp"
using namespace boost::numeric::ublas;
using namespace std;
int main(void)
{
	matrix<complex<double> > mat(3,3);
	matrix<complex<double> > gen(3,3);  // Generator of rotaion around z aix in group theory
	complex<double> img = std::complex<double>(0,1);
	gen(0,0) = 0  ; gen(0,1) = -img; gen(0,2) = 0;
	gen(1,0) = img; gen(1,1) = 0   ; gen(1,2) = 0;
	gen(2,0) = 0  ; gen(2,1) = 0   ; gen(2,2) = 0;

	double theta = 1.5;
	mat = img * theta * gen;
	cout<< "Rotation Matrix : "<< expm_pad(gen) <<"\n\n";
	return 0;
}


#include <Eigen/SparseCore>
#include <mathtoolbox/data-normalization.hpp>
#include <mathtoolbox/som.hpp>

namespace
{
    int GetNumNodes(const int resolution, const int latent_num_dims)
    {
        assert(latent_num_dims == 1 || latent_num_dims == 2);

        if (latent_num_dims == 1)
        {
            return resolution;
        }
        else
        {
            return resolution * resolution;
        }
    }

    /// \details The computational complexity is O(#data * #nodes)
    Eigen::VectorXi FindBestMatchingUnits(const Eigen::MatrixXd& X, const Eigen::MatrixXd& Y)
    {
        assert(X.rows() == Y.rows());

        const int num_data  = X.cols();
        const int num_nodes = Y.cols();

        Eigen::VectorXi node_indices(num_data);

        for (int i = 0; i < num_data; ++i)
        {
            Eigen::VectorXd squared_norms(num_nodes);

            for (int j = 0; j < num_nodes; ++j)
            {
                squared_norms(j) = (X.col(i) - Y.col(j)).squaredNorm();
            }

            squared_norms.minCoeff(&(node_indices(i)));
        }

        return node_indices;
    }

    Eigen::SparseMatrix<double> ConvertBestMatchingUnitsIntoMat(const Eigen::VectorXi& best_matching_units,
                                                                const int              num_nodes)
    {
        const int num_data = best_matching_units.size();

        Eigen::SparseMatrix<double> B(num_nodes, num_data);

        for (int data_index = 0; data_index < num_data; ++data_index)
        {
            B.insert(best_matching_units(data_index), data_index) = 1.0;
        }

        return B;
    }

    /// \return Posititons in the latent space [0, 1]^{#dims x #nodes}
    Eigen::MatrixXd GetLatentSpacePositions(const int resolution, const int latent_num_dims)
    {
        assert(latent_num_dims == 1 || latent_num_dims == 2);
        assert(resolution >= 2);

        const int num_nodes = GetNumNodes(resolution, latent_num_dims);

        Eigen::MatrixXd positions(latent_num_dims, num_nodes);

        if (latent_num_dims == 1)
        {
            for (int x_index = 0; x_index < num_nodes; ++x_index)
            {
                positions(0, x_index) = static_cast<double>(x_index) / static_cast<double>(resolution - 1);
            }
        }
        else
        {
            for (int y_index = 0; y_index < resolution; ++y_index)
            {
                for (int x_index = 0; x_index < resolution; ++x_index)
                {
                    const int index = y_index * resolution + x_index;

                    const double x_offset = static_cast<double>(x_index) / static_cast<double>(resolution - 1);
                    const double y_offset = static_cast<double>(y_index) / static_cast<double>(resolution - 1);

                    positions.col(index) = Eigen::Vector2d(x_offset, y_offset);
                }
            }
        }

        return positions;
    }

    Eigen::MatrixXd CalcNeighborhoodMat(const Eigen::MatrixXd& latent_node_positions,
                                        const int              iter_count,
                                        const double           init_var,
                                        const double           min_var,
                                        const double           var_decreasing_speed)
    {
        const int    num_nodes = latent_node_positions.cols();
        const double var       = std::max(init_var * std::exp(-iter_count / var_decreasing_speed), min_var);

        Eigen::MatrixXd H(num_nodes, num_nodes);

        for (int i = 0; i < num_nodes; ++i)
        {
            for (int j = i; j < num_nodes; ++j)
            {
                const double squared_dist = (latent_node_positions.col(i) - latent_node_positions.col(j)).squaredNorm();
                const double value        = std::exp(-(1.0 / (2.0 * var)) * squared_dist);

                H(i, j) = value;
                H(j, i) = value;
            }
        }

        return H;
    }
} // namespace

mathtoolbox::Som::Som(const Eigen::MatrixXd& data,
                      const int              latent_num_dims,
                      const int              resolution,
                      const double           init_var,
                      const double           min_var,
                      const double           var_decreasing_speed,
                      const bool             normalize_data)
    : m_latent_num_dims(latent_num_dims),
      m_resolution(resolution),
      m_init_var(init_var),
      m_min_var(min_var),
      m_var_decreasing_speed(var_decreasing_speed),
      m_normalize_data(normalize_data),
      m_latent_node_positions(GetLatentSpacePositions(resolution, latent_num_dims)),
      m_iter_count(0),
      m_X(data),
      m_data_normalizer(nullptr)
{
    if (m_normalize_data)
    {
        this->NormalizeData();
    }

    this->PerformInitialization();
}

Eigen::MatrixXd mathtoolbox::Som::GetDataSpaceNodePositions() const
{
    return m_normalize_data ? m_data_normalizer->Denormalize(m_Y) : m_Y;
}

void mathtoolbox::Som::Step()
{
    const int             num_nodes           = GetNumNodes(m_resolution, m_latent_num_dims);
    const int             num_data            = m_X.cols();
    const Eigen::VectorXi best_matching_units = FindBestMatchingUnits(m_X, m_Y);

    // #nodes * #data
    const Eigen::SparseMatrix<double> B = ConvertBestMatchingUnitsIntoMat(best_matching_units, num_nodes);

    // #nodes * #nodes
    const Eigen::MatrixXd H =
        CalcNeighborhoodMat(m_latent_node_positions, m_iter_count, m_init_var, m_min_var, m_var_decreasing_speed);

    // #nodes * #data
    const Eigen::MatrixXd R = H * B;

    // #nodes * #nodes
    Eigen::VectorXd G_inv_diag(num_nodes);
    for (int i = 0; i < num_nodes; ++i)
    {
        G_inv_diag(i) = 1.0 / R.row(i).sum();
    }
    const auto G_inv = Eigen::DiagonalMatrix<double, Eigen::Dynamic>(G_inv_diag);

    // Update Y (the positions of the grid nodes in the data space)
    m_Y = (G_inv * H * (B * m_X.transpose())).transpose();

    // Update Z (the positions of the data points in the latent space)
    for (int i = 0; i < num_data; ++i)
    {
        m_Z.col(i) = m_Y.col(best_matching_units(i));
    }

    // Update the iteration count
    ++m_iter_count;
}

void mathtoolbox::Som::NormalizeData()
{
    // Instantiate a data normalizer object
    m_data_normalizer = std::make_shared<const DataNormalizer>(m_X);

    // Replace X with its normalized version
    m_X = m_data_normalizer->GetNormalizedDataPoints();
}

void mathtoolbox::Som::PerformInitialization()
{
    const int num_data_dims = m_X.rows();
    const int num_data      = m_X.cols();
    const int num_nodes     = GetNumNodes(m_resolution, m_latent_num_dims);

    m_Y = Eigen::MatrixXd::Random(num_data_dims, num_nodes);
    m_Z = Eigen::MatrixXd::Random(num_data_dims, num_data);
}


#include <Eigen/Dense>
#include <iostream>
#include <stdio.h>
#include <cmath>
#include "pressiodemoapps/impl/swe_rusanov_flux_jacobian_function.hpp"
#include "pressiodemoapps/impl/swe_rusanov_flux_values_function.hpp"
#include <complex>

int main()
{
  auto passedString = "PASS";
  const double tol = 1e-12;
  const double eps = 1e-15;

  const int nVars = 3;
  using scalar_t = std::complex<double>;

  const scalar_t g = 9.8;
  const scalar_t hL = 0.4;
  const scalar_t uL = 8.;
  const scalar_t vL = 3.;

  Eigen::Matrix<scalar_t,1,-1> UL(nVars);
  UL(0) = hL;
  UL(1) = hL*uL;
  UL(2) = hL*vL;

  scalar_t hR = 0.125;
  scalar_t uR = -0.8;
  scalar_t vR = -0.3;
  Eigen::Matrix<scalar_t,1,-1> UR(nVars);
  UR(0) = hR;
  UR(1) = hR*uR;
  UR(2) = hR*vR;
  scalar_t normals[2];
  normals[0] = 1;
  normals[1] = 0;

  Eigen::Matrix<scalar_t,1,-1> flux(nVars);
  Eigen::Matrix<scalar_t,1,-1> fluxBase(nVars);
  Eigen::Matrix<scalar_t,-1,-1> JL(nVars,nVars);
  Eigen::Matrix<scalar_t,-1,-1> JR(nVars,nVars);
  Eigen::Matrix<scalar_t,-1,-1> JL_FD(nVars,nVars);
  Eigen::Matrix<scalar_t,-1,-1> JR_FD(nVars,nVars);
  pressiodemoapps::implswe::swe_rusanov_flux_jacobian_three_dof(JL,JR,UL,UR,normals,g);
  pressiodemoapps::implswe::swe_rusanov_flux_three_dof(fluxBase,UL,UR,normals,g);

  const scalar_t oneJ(0.0,eps);
  for (int i = 0;i < nVars; i++){
    UL(i) += oneJ;
    pressiodemoapps::implswe::swe_rusanov_flux_three_dof(flux,UL,UR,normals,g);
    for (int j = 0 ; j < nVars;j++){
      JL_FD(j,i) = 1./eps*(std::imag(flux(j)));
      if (std::abs( JL_FD(j,i) - JL(j,i) ) > tol) passedString = "FAILED";
    }
    UL(i) -= oneJ;

    UR(i) += oneJ;
    pressiodemoapps::implswe::swe_rusanov_flux_three_dof(flux,UL,UR,normals,g);
    for (int j = 0 ; j < nVars;j++){
      JR_FD(j,i) = 1./eps*std::imag(flux(j) );
      if (std::abs( JR_FD(j,i) - JR(j,i) ) > tol) passedString = "FAILED";
    }
    UR(i) -= oneJ;
  }

  normals[0] = 0;
  normals[1] = 1;
  pressiodemoapps::implswe::swe_rusanov_flux_jacobian_three_dof(JL,JR,UL,UR,normals,g);
  pressiodemoapps::implswe::swe_rusanov_flux_three_dof(fluxBase,UL,UR,normals,g);
  for (int i = 0;i < nVars; i++){
    UL(i) += oneJ;
    pressiodemoapps::implswe::swe_rusanov_flux_three_dof(flux,UL,UR,normals,g);
    for (int j = 0 ; j < nVars;j++){
      JL_FD(j,i) = 1./eps*std::imag(flux(j) );
      if (std::abs( JL_FD(j,i) - JL(j,i) ) > tol) passedString = "FAILED";
    }
    UL(i) -= oneJ;

    UR(i) += oneJ;
    pressiodemoapps::implswe::swe_rusanov_flux_three_dof(flux,UL,UR,normals,g);
    for (int j = 0 ; j < nVars;j++){
      JR_FD(j,i) = 1./eps*std::imag(flux(j));
      if (std::abs( JR_FD(j,i) - JR(j,i) ) > tol) passedString = "FAILED";
    }
    UR(i) -= oneJ;
  }

  std::cout << passedString << std::endl;

}


/************************************
 * ClassName: RSAUser
 * Function: 实现 RSA 加密使用者的类对象
 * *********************************/
#include "RSA.h"
#include "Random.h"
#include "AES.h"
#include <iostream>
#include <fstream>
#include <ctime>
#include <cstring>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <algorithm>

using namespace std;
using namespace NTL;

// _getch() 函数的系统移植问题解决
#ifdef _WIN32
#include <conio.h>
#elif __linux__
char _getch();
#endif

void printChoose() {
    cout << "Please choose the length of random prime: \n";
    cout << "1. 512  bits\n";
    cout << "2. 1024 bits\n";
}

RSAUser::RSAUser() {
    cout << "Create RSA user..." << endl;

}

void RSAUser::GenerateKey() {
    // 选择 RSA 素数的比特长度
    int m = 0;

    printChoose();
    do {
        char ch = _getch();
        switch(ch) {
            case '1': m = 8; break;
            case '2': m = 16; break;
            default: m = 0;
        }
    } while (m == 0);

    PrimeGen G(m);
    cout << "public key and private key are generating...\n";

    // 生成私钥的随机素数 p, q
    do {
        sk.p = G.GeneratePrime();
        sk.q = G.GeneratePrime();
    } while (sk.p == sk.q);

    // 生成公钥 n
    pk.n = sk.p * sk.q;
    ZZ Euler = (sk.p - 1) * (sk.q - 1);    // 欧拉函数数值

    do {
        pk.b = G.GenerateRandom() % Euler;
    } while(GCD(pk.b, Euler) != 1);

    // 使用 NTL 库中的求逆函数 InvModStatus()
    InvModStatus(sk.a, pk.b, Euler);
    cout << "All keys have been generated !\n";
    // 钥匙查看
    viewKey(m * 64);
}

// 发送 RSA 公钥
void RSAUser::SendPublicKey(RSAUser &B) {
    B.pk = this->pk;
}

// 创建临时密钥
void RSAUser::createTempKey() {
    PRNG G(2);
    k = G.GenerateRandom();
    cout << "Temp key has been created.\n";
    cout << "Do you want to view temp key, y/n ?\n";
    char ch = _getch();
    switch (ch) {
        case 'Y':
        case 'y':
            cout << "temp key\n";
            printKey(k, 128);
            break;
        default:
            break;
    }
}

// 加密信息，使用 CBC 模式
void RSAUser::EncryptMessage() {
    // 生成临时密钥
    createTempKey();
    // 利用公钥加密 k 得到 c1，此处使用 ZZ_p 类计算
    ZZ_p::init(pk.n);
    ZZ_p k_p = to_ZZ_p(k);
    M.c1 = rep(power(k_p, pk.b));

    // 将临时密钥 k 转化为 128 比特形式
    bitset<128> key(to_ulong(k));
    // 以此创建 AES 加密系统
    AES E(key);

    // 需要加密的文件路径输入
    string fileName, newfileName;
    cout << "Please input the file path: \n";
    cin >> fileName;
    _getch();   // 接收回车符

    // 密文存储为 源文件名 + ".cipher"
    newfileName = fileName + ".cipher";
    // 文件名作为信息传输
    M.fileName = fileName;

    // 打开文件
    ifstream fin(fileName, ios::binary);
    if (!fin) {
        cerr << "open file error.\n";
        exit(-1);
    }
    ofstream fout(newfileName, ios::binary);
    if (!fout) {
        cerr << "open file error.\n";
        exit(-1);
    }

    // 因为逐字符读取文件比较慢，使用缓存区的方式一次性读取 16*1024 字节
    bitset<128> buffer[512];
    // CBC 模式
    // former 记录密文 y(i-1) ，初始化为一个随机数
    PRNG G(2);
    M.IV = G.GenerateRandom();
    bitset<128> former(to_ulong(M.IV));
    // 将 IV 通过 RSA 加密发送
    ZZ_p IV_p = to_ZZ_p(M.IV);
    M.IV = rep(power(IV_p, pk.b));

    bitset<128> cipher;     // 密文

    // 开始读取需要加密的文件
    while (fin && !fin.eof()) {
        // 一次性读取最多 16*1024 字节的内容
        fin.read((char*)&buffer, 16*512);
        streamsize readNum = fin.gcount();
        streamsize i;
        for (i = 0; i < readNum / 16; i++) {
            cipher = E.AESEncrypt(former ^ buffer[i]);
            former = cipher;
            // 写入密文至文件中
            fout.write((char*)&cipher, 16);
        }
        streamsize end = readNum % 16;
        // 文件读取结束操作，使用 PKCS7Padding 处理末尾
        if (fin.eof()) {
            // 末尾填充的数字
            bitset<128> pkcs(16 - end);
            pkcs = pkcs << 120;
            for (streamsize j = 16; j > end; j--) {
                streamsize k = j - end - 1;
                buffer[i] = ((buffer[i] << k*8) ^ pkcs) >> k*8;
            }
            cipher = E.AESEncrypt(former ^ buffer[i]);
            fout.write((char*)&cipher, 16);
        }
    }

    cout << "File encryption finished.\n";
    cout << "Encrypted file is located at " << newfileName << endl;
    // 关闭文件
    fin.close();
    fout.close();
}

// 解密信息
void RSAUser::DecryptMessage() {
    // 利用密钥来解密 c1，得到临时密钥 k
    ZZ_p::init(pk.n);
    ZZ_p c1_p = to_ZZ_p(M.c1);
    k = rep(power(c1_p, sk.a));
    // 解密得到 IV
    ZZ_p IV_p = to_ZZ_p(M.IV);
    M.IV = rep(power(IV_p, sk.a));

    // 查看临时密钥
    cout << "Temp key has been decrypted.\n";
    cout << "Do you want to view temp key, y/n ?\n";
    char ch = _getch();
    switch (ch) {
        case 'Y':
        case 'y':
            cout << "temp key\n";
            printKey(k, 128);
            break;
        default:
            break;
    }

    // 使用密钥 k 解密
    bitset<128> key(to_ulong(k));
    // 创建 AES 加密解密系统
    AES D(key);

    // 密文文件
    string fileName = M.fileName + ".cipher";

    // 查找文件的名称中的 '\' 或者 '/' 最后出现位置
    int pos = M.fileName.length() - 1;
    for (; pos >= 0; pos--) {
        if (M.fileName[pos] == '/' || M.fileName[pos] == '\\')
            break;
    }
    // 解密文件位置
    string newfileName = M.fileName.insert(pos + 1, "new_");

    // 打开文件
    ifstream fin(fileName, ios::binary);
    if (!fin) {
        cerr << "open file error.\n";
        exit(-1);
    }
    ofstream fout(newfileName, ios::binary);
    if (!fout) {
        cerr << "open file error.\n";
        exit(-1);
    }

    // buffer 缓冲区读取
    bitset<128> buffer[512];
    // former 记录密文 C(i-1)，初始化为 M.IV
    bitset<128> former(to_ulong(M.IV));
    bitset<128> plain;      // 明文

    // 开始读取解密文件
    while (fin && !fin.eof()) {
        fin.read((char*)&buffer, 16*512);
        streamsize readNum = fin.gcount();

        bool flag = false;  // 判断是否结束的标志
        if (fin.eof() || fin.peek() == EOF) {
            flag = true;
        }
        streamsize i, bufferMax = readNum / 16;
        // 使用 end 记录需要解密的缓冲区最大编号
        if (flag) bufferMax--;

        for (i = 0; i < bufferMax; i++) {
            plain = D.AESDecrypt(buffer[i]) ^ former;
            former = buffer[i];
            // 写入解密文件
            fout.write((char*)&plain, 16);
        }
        // 处理 PKCS7Padding 结尾
        if (flag) {
            // end 表示额外填充的字节数
            // 最后一次解密
            plain = D.AESDecrypt(buffer[i]) ^ former;
            streamsize end = (plain >> 120).to_ulong();
            for (streamsize j = 16; j > end; j--) {
                // 最后的字节处理
                bitset<8> text((plain << (j-1)*8 >> 120).to_ulong());
                fout.write((char*)&text, 1);
            }
        }
    }

    cout << "File decryption finished.\n";
    cout << "Decrypted file is located at " << newfileName << endl;
    fin.close();
    fout.close();
}

// 发送信息
void RSAUser::SendMessage(RSAUser& A) {
    A.M = this->M;
}

// PEM 格式打印输出
void RSAUser::PrintInPEM(const string s) {
	string res = "";
	size_t len = s.length() / 8;	// 比特串转化为字节的长度
	size_t i;
	unsigned char triBytes[3];		// 存储三个字节

	for (i = 0; i+3 <= len; i += 3) {
		for (int j = 0; j < 3; j++) {
			bitset<8> tmp(s.substr(8*i + 8*j, 8));
			triBytes[j] = tmp.to_ulong();
		}
		res += Base64Map[triBytes[0] >> 2];
		res += Base64Map[((triBytes[0]<<4) & 0x30) | (triBytes[1] >> 4)];
		res += Base64Map[((triBytes[1]<<2) & 0x3c) | (triBytes[2] >> 6)];
		res += Base64Map[triBytes[2] & 0x3f];
	}

	if (i < len) {
		if (len - i == 1) {
			bitset<8> tmp(s.substr(8*i, 8));
			triBytes[0] = tmp.to_ulong();
			res += Base64Map[triBytes[0] >> 2];
			res += Base64Map[(triBytes[0]<<4) & 0x30];
			res += "==";
		} else {
			for (int j = 0; j < 2; j++) {
				bitset<8> tmp(s.substr(8*i + 8*j, 8));
				triBytes[j] = tmp.to_ulong();
			}
			res += Base64Map[triBytes[0] >> 2];
			res += Base64Map[((triBytes[0]<<4) & 0x30) | (triBytes[1] >> 4)];
			res += Base64Map[(triBytes[1]<<2) & 0x3c];
			res += "=";
		}
	}

	cout << res << endl;
}

// DER(十六进制)格式打印
void RSAUser::PrintInDER(const string s) {
    cout << "modulus:";
    string pairstr = "";
    for (size_t i = 0; i < s.length(); i += 4) {
        int index = 0;

        for (size_t j = i; j < i + 4; j++) {
            index = index << 1;
            if (s[j] == '1') {
                index += 1;
            }
        }
        pairstr += HexTable[index];
        if (pairstr.length() == 2) {
            cout << pairstr;
            if (i + 4 < s.length()) {
                cout << ":";
            }
            pairstr = "";
        }
        if (i % 120 == 0) {
            cout << endl << '\t';
        }
    }
    cout << endl;
}

// ZZ 类型转化为 Bits
string RSAUser::ZZToBits(ZZ num, const size_t n) {
	string s = "";
	ZZ last;

	while (num != 0) {
		last = num % 2;
		if (last == 1) {
			s += '1';
		} else {
			s += '0';
		}
		num /= 2;
	}
	for (size_t i = s.length(); i < n; i++) {
		s += '0';
	}
	reverse(s.begin(), s.end());
	return s;
}

// 选择输出格式打印
void printFormat() {
    cout << "Please choose the format to print key :\n";
    cout << "1. DER (hexadecimal)\n";
    cout << "2. PEM\n";
}

// 查看钥匙，n 为 p,q 的比特位数
void RSAUser::viewKey(const size_t n) {
    // 查看公钥
    cout << "Do you want to view public key, y/n ?\n";
    char ch = _getch();

    switch(ch) {
        case 'Y':
        case 'y': {
            cout << "Public key\n";
            cout << "Print n in public key :\n";
            printKey(pk.n, n*2);
            cout << "Print b in public key :\n";
            printKey(pk.b, n*2);
            break;
        }
        default: break;
    }
    // 查看私钥
    cout << "Do you want to view private key, y/n ?\n";
    ch = _getch();

    if (ch == 'y' || ch == 'Y') {
        cout << "Attention, please not reveal this information !!!\n";
        cout << "Read the warning, continue, y/n ?\n";
        ch = _getch();

        switch(ch) {
            case 'Y':
            case 'y': {
                cout << "Private key\n";
                cout << "Print p in private key :\n";
                printKey(sk.p, n);
                cout << "Print q in private key :\n";
                printKey(sk.q, n);
                cout << "Print a in private key :\n";
                printKey(sk.a, n*2);
                break;
            }
            default: break;
        }
    }

}

// 打印每一个钥匙
void RSAUser::printKey(ZZ num, const size_t n) {
    string s = ZZToBits(num, n);
    printFormat();
    bool flag = false;
    while (!flag) {
        char ch = _getch();
        switch(ch) {
            case '1': {
                PrintInDER(s);
                flag = true;
                break;
            }
            case '2': {
                PrintInPEM(s);
                flag = true;
                break;
            }
            default: break;
        }
    }
}


/*!
 * \file AffHypPlane.hpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Descember 9, 2019: created
 */

#pragma once

// #include <iostream> // only for Debug
#include <utility>
#include <tuple>
#include <array>
#include <Eigen/Dense>

#include "../utils.hpp"

template<size_t n>
class AffHypPlane
{
public:
    /*static constexpr size_t*/ enum : size_t { dim = n };
    static constexpr double threshold = 1e-14;
    using VecT = typename Eigen::Matrix<double,dim,1>;

private:
    //! The defining function of the line is
    //!   <m_normal|x> + m_c = 0;
    VecT m_normal;
    double m_c;

public:
    AffHypPlane(std::array<double,dim> const & normal, double c)
        : m_normal(Eigen::Map<const VecT>(&normal[0], dim)), m_c(c)
    {}

    AffHypPlane(VecT const &normal, VecT const &refpt)
        : m_normal(normal), m_c(-normal.dot(refpt))
    {}

    AffHypPlane(AffHypPlane<n> const &) = default;
    AffHypPlane(AffHypPlane<n> &&) = default;

    //! Compute an intersection with another hyper plane.
    //! The function is specialized in case of dimension 2.
    template <size_t m= dim>
    auto intersect(AffHypPlane<m> const &another) const
        -> std::pair<bool,VecT>
    {
        Eigen::Matrix<double,2,dim> A;
        A.row(0) = m_normal;
        A.row(1) = another.m_normal;
        Eigen::Vector2d b{-m_c, -another.m_c};

        Eigen::Matrix<double,dim,1> x = A.colPivHouseholderQr().solve(b);

        return {(A*x-b).norm()/(b.norm()+m_normal.norm()) < threshold, x};
    }

    //! Compute the height of a given point from the hyperplane.
    //! \warning: The value is not normalized; or with respect to the normal vector of the hyperplane.
    double height(VecT const &v) const
    {
        return m_normal.dot(v) + m_c;
    }

    template <class... Ts>
    double height(double x, Ts... xs) const
    {
        static_assert(1+sizeof...(Ts)==dim, "Wrong number of arguments.");
        static_assert(
            bord2::allTrue({std::is_convertible<Ts,double>::value...}),
            "The arguments must be convertible to double." );
        return height(VecT(x, xs...));
    }
};

template<size_t n>
constexpr double AffHypPlane<n>::threshold;


#pragma once
#include <Eigen/Dense>
#include <chrono>
#include <vector>

class Kalman
{
public:
    struct Inputs
    {
        double eastAcc;
        double northAcc;
    };

    struct Measurements
    {
        double x;
        double y;
    };

    typedef Measurements Estimates;

private:
    typedef std::chrono::time_point<std::chrono::high_resolution_clock> timeVar;

    timeVar previousUpdateTime = std::chrono::high_resolution_clock::now();

    const std::vector<double> processVariance;
    const std::vector<double> measurementVariance;

    bool fstScan = true;
    Eigen::Vector2d aPosteriori_xHat;
    Eigen::Matrix2d aPosteriori_P;

public:
    Kalman(const std::vector<double> &_processVariance, const std::vector<double> &_measurementVariance);
    Estimates update(const Inputs &inputs, const Measurements &measurements);
    Estimates getEstimates() const;
};


#pragma once

#include <Eigen/Eigen>
#include <unsupported/Eigen/KroneckerProduct>
#include <array>
#include <memory>
#include <tuple>

template<typename Scalar, int Order>
class Tensor
{
public:
	using SizeVector = std::array<int, Order>;

	Tensor() noexcept : m_size{}, m_offsets{}, m_numElements(0), m_capacity(0), m_data(nullptr) {}

	explicit Tensor(const SizeVector& _size)
		: m_size(_size)
	{
		computeOffsets();
		m_numElements = static_cast<size_t>(m_offsets.back())
			* static_cast<size_t>(m_size.back());
		m_capacity = m_numElements;

		m_data = std::make_unique<Scalar[]>(m_numElements);
	}

	Tensor(const SizeVector& _size, const Scalar* _data)
		: Tensor(_size)
	{
		if (_data)
			std::copy(_data, _data + m_numElements, m_data.get());
	}

	Tensor(const Tensor& _oth)
		: m_size(_oth.m_size),
		m_offsets(_oth.m_offsets),
		m_numElements(_oth.m_numElements),
		m_capacity(_oth.m_numElements),
		m_data(std::make_unique<Scalar[]>(m_numElements))
	{
		std::copy(_oth.m_data.get(), _oth.m_data.get() + m_numElements, m_data.get());
	}

	Tensor(Tensor&& _oth) noexcept
		: m_size(_oth.m_size),
		m_offsets(_oth.m_offsets),
		m_numElements(_oth.m_numElements),
		m_capacity(_oth.m_capacity),
		m_data(std::move(_oth.m_data))
	{}

	template<typename StreamT>
	explicit Tensor(StreamT& _stream)
	{
		_stream.read(reinterpret_cast<char*>(m_size.data()), m_size.size() * sizeof(int));

		computeOffsets();
		m_numElements = static_cast<size_t>(m_offsets.back())
			* static_cast<size_t>(m_size.back());
		m_capacity = m_numElements;

		m_data = std::make_unique<Scalar[]>(m_numElements);

		_stream.read(reinterpret_cast<char*>(m_data.get()), m_numElements * sizeof(Scalar));
	}

	Tensor& operator=(const Tensor& _oth)
	{
		m_size = _oth.m_size;
		m_offsets = _oth.m_offsets;
		m_numElements = _oth.m_numElements;
		m_capacity = m_numElements;
		m_data = std::make_unique<Scalar[]>(m_numElements);
		std::copy(_oth.m_data.get(), _oth.m_data.get() + m_numElements, m_data.get());

		return *this;
	}

	Tensor& operator=(Tensor&& _oth) noexcept
	{
		m_size = _oth.m_size;
		m_offsets = _oth.m_offsets;
		m_numElements = _oth.m_numElements;
		m_capacity = _oth.m_capacity;
		m_data = std::move(_oth.m_data);

		return *this;
	}

	// set from a k-flattening
	void set(const Eigen::MatrixX<Scalar>& _flatTensor, int _k)
	{
		assert(_flatTensor.rows() == m_size[_k]);
		assert(_flatTensor.rows() * _flatTensor.cols() == m_numElements);

		for (size_t i = 0; i < m_numElements; ++i)
		{
			const auto& [indK, indOth] = decomposeFlatIndex(i, _k);
			m_data[i] = _flatTensor(indK, indOth);
		}
	}

	//set from a k-flattening with K known at compile time
	template<int K>
	void set(const Eigen::MatrixX<Scalar>& _flatTensor) noexcept
	{
		static_assert(K < Order);
		assert(_flatTensor.rows() == m_size[K]);
		assert(_flatTensor.rows() * _flatTensor.cols() == m_numElements);

		if constexpr (K == 0)
		{
			std::copy(_flatTensor.data(), _flatTensor.data() + m_numElements, m_data.get());
			return;
		}

		for (size_t i = 0; i < m_numElements; ++i)
		{
			const auto& [indK, indOth] = decomposeFlatIndex<K>(i);
			m_data[i] = _flatTensor(indK, indOth);
		}
	}

	template<typename Gen>
	void set(Gen _generator)
	{
		for(size_t i = 0; i < m_numElements; ++i)
			m_data[i] = _generator(index(i));
	}

	void append(const Tensor<Scalar, Order>& _tensor)
	{
		for (int i = 0; i < Order - 1; ++i)
			if (m_size[i] != _tensor.size()[i])
				throw std::string("Incompatible tensor sizes.");

		reserve(m_numElements + _tensor.numElements());
		std::copy(_tensor.data(), _tensor.data() + _tensor.numElements(), m_data.get() + m_numElements);
		m_size.back() += _tensor.size().back();
		m_numElements += _tensor.numElements();
	}

	Eigen::MatrixX<Scalar> flatten(int _k) const
	{
		const size_t othDim = m_numElements / m_size[_k];
		Eigen::MatrixX<Scalar> m(m_size[_k], othDim);

		for (size_t i = 0; i < m_numElements; ++i)
		{
			const auto& [indK, indOth] = decomposeFlatIndex(i, _k);
			m(indK, indOth) = m_data[i];
		}

		return m;
	}

	// If K is known at compile time use this.
	template<int K>
	Eigen::MatrixX<Scalar> flatten() const
	{
		const size_t othDim = m_numElements / m_size[K];
		Eigen::MatrixX<Scalar> m(m_size[K], othDim);

		if constexpr (K == 0)
		{
			std::copy(m_data.get(), m_data.get() + m_numElements, m.data());
		}
		else
		{
			for (size_t i = 0; i < m_numElements; ++i)
			{
				const auto& [indK, indOth] = decomposeFlatIndex<K>(i);
				m(indK, indOth) = m_data[i];
			}
		}

		return m;
	}

	// Change the size of this tensor to _newSize.
	// The data is unspecified afterwards.
	// @param _shrink Shrink the buffer if the new size is smaller.
	void resize(const SizeVector& _newSize, bool _shrink = false)
	{
		m_size = _newSize;
		const std::size_t oldNum = m_numElements;

		computeOffsets();
		m_numElements = static_cast<size_t>(m_offsets.back())
			* static_cast<size_t>(m_size.back());

		if (m_capacity < m_numElements || (_shrink && oldNum > m_numElements))
		{
			m_data = std::make_unique<Scalar[]>(m_numElements);
			m_capacity = m_numElements;
		}
	}

	// Ensures that the reserved memory can hold atleast _capacity elements.
	// If the buffer is already larger no allocations take place.
	void reserve(std::size_t _capacity)
	{
		if (_capacity <= m_capacity) return;

		Scalar* newData = new float[_capacity];
		std::copy(m_data.get(), m_data.get() + m_numElements, newData);
		m_data.reset(newData);
		m_capacity = _capacity;
	}

	// ACCESS OPERATIONS

	// vectorization
	Eigen::Map<const Eigen::VectorX<Scalar>> vec() const noexcept
	{
		return { m_data.get(), static_cast<Eigen::Index>(m_numElements) };
	}

	// view which is equivalent to the 0-flattening
	Eigen::Map<const Eigen::MatrixX<Scalar>> mat() const noexcept
	{
		return { m_data.get(),
			static_cast<Eigen::Index>(m_size[0]),
			static_cast<Eigen::Index>(m_numElements / m_size[0]) };
	}

	// index access
	Scalar& operator[](const SizeVector& _index) noexcept { return m_data[flatIndex(_index)]; }
	Scalar operator[](const SizeVector& _index) const noexcept { return m_data[flatIndex(_index)]; }

	// raw access to the underlying memory
	Scalar* data() noexcept { return m_data.get(); }
	const Scalar* data() const noexcept { return m_data.get(); }

	constexpr int order() const noexcept { return Order; }
	const SizeVector& size() const noexcept { return m_size; }
	const std::size_t numElements() const noexcept { return m_numElements; }

	template<int OthOrder>
	bool isSameSize(const Tensor<Scalar, OthOrder>& _oth) const noexcept
	{
		if constexpr(OthOrder != Order) return false;

		for (int i = 0; i < Order; ++i)
			if (m_size[i] != _oth.m_size[i]) return false;

		return true;
	}

	bool operator==(const Tensor& _oth) const noexcept
	{
		if (!isSameSize(_oth)) return false;

		return std::memcmp(m_data.get(), _oth.m_data.get(), m_numElements * sizeof(Scalar)) == 0;
	}

	// ARITHMETIC OPERATORS
	Tensor<Scalar, Order> operator+(const Tensor<Scalar, Order>& _oth) const
	{
		assert(isSameSize(_oth));

		Tensor<Scalar, Order> tensor(m_size);
		for (size_t i = 0; i < m_numElements; ++i)
		{
			tensor.m_data[i] = m_data[i] + _oth.m_data[i];
		}

		return tensor;
	}

	Tensor<Scalar, Order> operator-(const Tensor<Scalar, Order>& _oth) const
	{
		assert(isSameSize(_oth));

		Tensor<Scalar, Order> tensor(m_size);
		for (size_t i = 0; i < m_numElements; ++i)
		{
			tensor.m_data[i] = m_data[i] - _oth.m_data[i];
		}

		return tensor;
	}

	// Frobenius Norm
	Scalar norm() const noexcept
	{
		Scalar s = 0;
		for (size_t i = 0; i < m_numElements; ++i)
			s += m_data[i] * m_data[i];

		return std::sqrt(s);
	}

	size_t flatIndex(const SizeVector& _index) const noexcept
	{
		std::size_t flatInd = _index[0];
		std::size_t dimSize = m_size[0];
		for (std::size_t i = 1; i < _index.size(); ++i)
		{
			flatInd += dimSize * _index[i];
			dimSize *= m_size[i];
		}

		return flatInd;
	}

	SizeVector index(size_t _flatIndex) const noexcept
	{
		SizeVector ind{};
		size_t reminder = _flatIndex;

		for (std::size_t i = 0; i < m_size.size(); ++i)
		{
			ind[i] = reminder % m_size[i];
			reminder /= m_size[i];
		}

		return ind;
	}

	// SERIALIZATION
	template<typename StreamT>
	void save(StreamT& _stream) const
	{
		_stream.write(reinterpret_cast<const char*>(m_size.data()), m_size.size() * sizeof(int));
		_stream.write(reinterpret_cast<const char*>(m_data.get()),
			m_numElements * sizeof(Scalar));
	}
private:

	void computeOffsets() noexcept
	{
		m_offsets[0] = 1;
		for (std::size_t i = 1; i < m_size.size(); ++i)
		{
			m_offsets[i] = m_offsets[i-1] * m_size[i-1];
		}
	}

	// Compute new indices for a k-flattening from a flatIndex.
	std::pair<size_t, size_t> decomposeFlatIndex(size_t flatIndex, int _k) const noexcept
	{
		SizeVector ind{};
		size_t reminder = flatIndex;

		std::size_t flatInd = 0;
		std::size_t dimSize = 1;
		for (int j = 0; j < _k; ++j)
		{
			ind[j] = reminder % m_size[j];
			reminder /= m_size[j];

			flatInd += dimSize * ind[j];
			dimSize *= m_size[j];
		}

		ind[_k] = reminder % m_size[_k];
		reminder /= m_size[_k];

		flatInd += reminder * dimSize;

		return { ind[_k], flatInd };
	}

	// variant for compile time K
	template<int K>
	std::pair<size_t, size_t> decomposeFlatIndex(size_t flatIndex) const noexcept
	{
		static_assert(K < Order);

		SizeVector ind{};
		size_t reminder = flatIndex;

		std::size_t flatInd = 0;
		std::size_t dimSize = 1;
		for (int j = 0; j < K; ++j)
		{
			ind[j] = reminder % m_size[j];
			reminder /= m_size[j];

			flatInd += dimSize * ind[j];
			dimSize *= m_size[j];
		}

		ind[K] = reminder % m_size[K];
		reminder /= m_size[K];

		flatInd += reminder * dimSize;

		return { ind[K], flatInd };
	}

	SizeVector m_size;
	SizeVector m_offsets; // cumulative sizes
	std::size_t m_numElements;
	std::size_t m_capacity;
	std::unique_ptr<Scalar[]> m_data;
};

namespace details {
	template<int K, typename Scalar, int Order, std::size_t OrderA>
	void multilinearProductImpl(const std::array<Eigen::MatrixX<Scalar>, OrderA>& _matrices,
		Tensor<Scalar, Order>& _tensor,
		bool _transpose)
	{
		static_assert(Order == OrderA);
		{
			Eigen::MatrixX<Scalar> flat = _tensor.template flatten<K>();
			if (_transpose)
				flat = _matrices[K].transpose() * flat;
			else
				flat = _matrices[K] * flat;

			auto sizeVec = _tensor.size();
			sizeVec[K] = static_cast<int>(flat.rows());
			_tensor.resize(sizeVec);
			_tensor.template set<K>(flat);
		}
		if constexpr (K < Order - 1)
			details::multilinearProductImpl<K + 1>(_matrices, _tensor, _transpose);
	}
}

// Multilinear product via k-flattening
// @param _transpose If true, the matrices are multiplied transposed with the tensor.
template<typename Scalar, int Order, std::size_t OrderS>
auto multilinearProduct(const std::array<Eigen::MatrixX<Scalar>, OrderS>& _matrices,
	const Tensor<Scalar, Order>& _tensor,
	bool _transpose = false)
	-> Tensor<Scalar, Order>
{
	static_assert(OrderS == Order);

	auto result = _tensor;

	details::multilinearProductImpl<0>(_matrices, result, _transpose);

	return result;
}

// Multilinear product via Kronecker product
// This method requires massive amounts of memory and should not be used.
template<typename Scalar>
auto multilinearProductKronecker(const std::array<Eigen::MatrixX<Scalar>, 3>& _matrices,
	const Tensor<Scalar, 3>& _tensor,
	bool _transpose = false)
	-> Tensor<Scalar, 3>
{
	const Eigen::VectorX<Scalar> core = _transpose ? (kroneckerProduct(_matrices[2].transpose(), kroneckerProduct(_matrices[1].transpose(), _matrices[0].transpose())) * _tensor.vec()).eval()
		: (kroneckerProduct(_matrices[2], kroneckerProduct(_matrices[1], _matrices[0])) * _tensor.vec()).eval();

	typename Tensor<Scalar, 3>::SizeVector sizeVec;
	for (size_t i = 0; i < sizeVec.size(); ++i)
		sizeVec[i] = static_cast<int>(_matrices[i].rows());

	return Tensor<Scalar, 3>(sizeVec, core.data());
}


#include <filter/KalmanFilter.h>

#include <Eigen/QR>
#include <cmath>

KalmanFilter::KalmanFilter(ParamsManager* params) {
  params_ = params;

  // Init State vector
  state_t_.setZero();
  state_t_1_.setZero();

  // Set constant velocity
  // TODO(alericcardi): create const variable
  state_t_(3, 0) = params_->init_acceleration[0];
  state_t_(4, 0) = params_->init_acceleration[1];
  state_t_(5, 0) = params_->init_acceleration[2];

  // Init Covariance noise
  P_.setZero();

  // Set Covariance process noise
  D_.setZero();
  // TODO(alericcardi): create const variable
  D_(0, 0) = std::pow(params_->noise_process[0], 2);
  D_(1, 1) = std::pow(params_->noise_process[1], 2);
  D_(2, 2) = std::pow(params_->noise_process[2], 2);

  // Set Covariance GPS observation noise
  R_gps_.setZero();
  R_gps_(0, 0) = std::pow(params_->noise_gps[0], 2);
  R_gps_(1, 1) = std::pow(params_->noise_gps[1], 2);
  R_gps_(2, 2) = std::pow(params_->noise_gps[2], 2);

  // Set Covariance Radar observation noise
  R_radar_.setZero();
  R_radar_(0, 0) = std::pow(params_->noise_radar[0], 2);
  R_radar_(1, 1) = std::pow(params_->noise_radar[1], 2);
  R_radar_(2, 2) = std::pow(params_->noise_radar[2], 2);
}

void KalmanFilter::initialization_step(const GPS_DATA& gps1) {
  // Save the current timestamp
  cur_time_ = gps1.timestamp;

  // Set the state as the initial GPS position
  state_t_.block<3, 1>(0, 0) << gps1.pose;
}

void KalmanFilter::propagation_step(double timestamp) {
  // ---------------------------------------------------------------------------
  // Compute the Velocity

  if (dt_ > 0.1) {
    state_t_.tail(3) =
        (state_t_.head(3) - state_t_1_.head(3)) / (dt_ + 0.00001);
    state_t_1_ = state_t_;
  }

  // ---------------------------------------------------------------------------
  // Update the time offset (dt)

  dt_ = timestamp - cur_time_;
  cur_time_ = timestamp;

  // ---------------------------------------------------------------------------
  // Update the State-transition matrix
  update_F();

  // ---------------------------------------------------------------------------
  // Propagate the state and coovariance

  state_t_ = F_ * state_t_;
  P_ = F_ * P_ * F_.transpose() + D_;
}

void KalmanFilter::correction_gps(GPS_DATA& gps_m) {
  Eigen::Matrix<double, 6, 6> H_t = compute_H_gps();
  Eigen::Matrix<double, 6, 1> Y_t = Eigen::Matrix<double, 6, 1>::Zero();

  // Compute the residual between GPS and State
  Y_t.topLeftCorner(3, 1) = gps_m.pose - state_t_.head(3);

  correction_step(Y_t, H_t, R_gps_);
}

void KalmanFilter::correction_radar(RADAR_DATA& radar_m) {
  Eigen::Matrix<double, 6, 6> H_t = compute_H_radar();
  Eigen::Matrix<double, 6, 1> Y_t = Eigen::Matrix<double, 6, 1>::Zero();

  // Compute the residual between Radar and State
  Y_t.head(3) = radar_m.beam - h_radar();

  correction_step(Y_t, H_t, R_radar_);
}

Eigen::Vector3d KalmanFilter::get_state() { return state_t_.head(3); }

void KalmanFilter::correction_step(Eigen::Matrix<double, 6, 1> Y_t,
                                   Eigen::Matrix<double, 6, 6> H_t,
                                   Eigen::Matrix<double, 6, 6> R_t) {
  Eigen::Matrix<double, 6, 6> S_t;
  Eigen::Matrix<double, 6, 6> S_t_inv;
  Eigen::Matrix<double, 6, 6> K_t;

  // ---------------------------------------------------------------------------
  S_t = H_t * P_ * H_t.transpose() + R_t;
  S_t_inv = S_t.completeOrthogonalDecomposition().pseudoInverse();
  K_t = P_ * H_t.transpose() * S_t_inv;

  // ---------------------------------------------------------------------------
  // Correct state and coovariance
  state_t_ = state_t_ + K_t * Y_t;
  P_ = (Eigen::Matrix<double, 6, 6>::Identity() - K_t * H_t) * P_;
  // P_ = P_ - K_t * S_t * K_t.transpose();
}

void KalmanFilter::update_F() {
  // Init of the State-transistion matrix
  F_.block<1, 6>(0, 0) << 1, 0, 0, dt_, 0, 0;
  F_.block<1, 6>(1, 0) << 0, 1, 0, 0, dt_, 0;
  F_.block<1, 6>(2, 0) << 0, 0, 1, 0, 0, dt_;
  F_.block<1, 6>(3, 0) << 0, 0, 0, 1, 0, 0;
  F_.block<1, 6>(4, 0) << 0, 0, 0, 0, 1, 0;
  F_.block<1, 6>(5, 0) << 0, 0, 0, 0, 0, 1;
}

Eigen::Matrix<double, 6, 6> KalmanFilter::compute_H_gps() {
  Eigen::Matrix<double, 6, 6> H_t = Eigen::Matrix<double, 6, 6>::Zero();

  H_t.block<3, 3>(0, 0) << Eigen::Matrix3d::Identity();

  return H_t;
}

Eigen::Matrix<double, 6, 6> KalmanFilter::compute_H_radar() {
  Eigen::Matrix<double, 6, 6> H_t = Eigen::Matrix<double, 6, 6>::Zero();
  double x = state_t_(0, 0) - params_->init_pose_radar(0, 0);
  double y = state_t_(1, 0) - params_->init_pose_radar(1, 0);
  double z = state_t_(2, 0) - params_->init_pose_radar(2, 0);
  double x_2 = std::pow(x, 2);
  double y_2 = std::pow(y, 2);
  double z_2 = std::pow(z, 2);
  double ro = std::pow(x_2 + y_2 + z_2, 0.5);
  double ro_2 = std::pow(ro, 2);
  double sqrt_x_2_y_2 = std::pow(x_2 + y_2, 0.5);

  H_t.block<1, 3>(0, 0) << x / ro, y / ro, z / ro;
  H_t.block<1, 3>(1, 0) << (x * z) / (ro_2 * sqrt_x_2_y_2),
      (y * z) / (ro_2 * sqrt_x_2_y_2), (-sqrt_x_2_y_2 / ro_2);
  H_t.block<1, 3>(2, 0) << (-y / (x_2 + y_2)), (x / (x_2 + y_2)), 0;

  return H_t;
}

Eigen::Vector3d KalmanFilter::h_radar() {
  Eigen::Vector3d z_radar;
  // From global to local coordinates
  double x = state_t_(0, 0) - params_->init_pose_radar(0, 0);
  double y = state_t_(1, 0) - params_->init_pose_radar(1, 0);
  double z = state_t_(2, 0) - params_->init_pose_radar(2, 0);

  // From local cartesian coordinates to local spherical coordinates
  z_radar(0, 0) =
      std::pow(std::pow(x, 2) + std::pow(y, 2) + std::pow(z, 2), 0.5);
  z_radar(1, 0) = std::acos(z / z_radar(0, 0));
  z_radar(2, 0) = std::atan(y / (x + 0.0001));

  return z_radar;
}


#include "gps_ba_helper.h"
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include "pointstoply.h"
#include <json/json.h>
#include <spdlog/spdlog.h>
#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <openvslam/camera/perspective.h>
#include <openvslam/data/frame.h>
#include <openvslam/data/keyframe.h>
#include <openvslam/data/landmark.h>

class PoseGraph3dPoseErrorTerm {
public:
    PoseGraph3dPoseErrorTerm(const Eigen::Vector3d& measured)
        : P_measured_(measured) {}

    template<typename T>
    bool operator()(const T* const p_ptr, const T* const q_a_ptr, T* residuals_ptr) const {
        Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals(residuals_ptr);
        Eigen::Map<const Eigen::Matrix<T, 3, 1>> P(p_ptr);
        Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
        Eigen::Matrix<T, 3, 1> P_glob = -(q_a.conjugate() * P);
        residuals = P_glob - P_measured_.template cast<T>();

        return true;
    }

    static ceres::CostFunction* Create(const Eigen::Vector3d& P_measured) {
        return new ceres::AutoDiffCostFunction<PoseGraph3dPoseErrorTerm, 3, 3, 4>(
            new PoseGraph3dPoseErrorTerm(P_measured));
    }

private:
    // The measurement for the position of B relative to A in the A frame.
    const Eigen::Vector3d P_measured_;
};

struct SnavelyReprojectionError {
    SnavelyReprojectionError(double observed_x, double observed_y,
                             double fx, double fy, double cx, double cy)
        : observed_x(observed_x), observed_y(observed_y), fx(fx), fy(fy), cx(cx), cy(cy) {}

    template<typename T>
    bool operator()(const T* const q_curr_glob,
                    const T* const p_curr_glob,
                    const T* const point,
                    T* residuals) const {
        // camera[0,1,2] are the angle-axis rotation.
        Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(p_curr_glob);
        Eigen::Map<const Eigen::Quaternion<T>> q_a(q_curr_glob);
        Eigen::Map<const Eigen::Matrix<T, 3, 1>> P(point);

        Eigen::Matrix<T, 3, 1> p = q_a * P + t;

        // Compute the center of distortion. The sign change comes from
        // the camera model that Noah Snavely's Bundler assumes, whereby
        // the camera coordinate system has a negative z axis.
        T xp = p[0] / p[2];
        T yp = p[1] / p[2];

        T predicted_x = fx * xp + cx;
        T predicted_y = fy * yp + cy;

        // The error is the difference between the predicted and observed position.
        residuals[0] = predicted_x - T(observed_x);
        residuals[1] = predicted_y - T(observed_y);
        return true;
    }

    // Factory to hide the construction of the CostFunction object from
    // the client code.
    static ceres::CostFunction* Create(const double observed_x,
                                       const double observed_y,
                                       const double fx, const double fy, const double cx, const double cy) {
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 4, 3, 3>(
            new SnavelyReprojectionError(observed_x, observed_y, fx, fy, cx, cy)));
    }

    double observed_x;
    double observed_y;
    double fx;
    double fy;
    double cx;
    double cy;
};

class PoseGraph3dErrorTerm {
public:
    PoseGraph3dErrorTerm(const Eigen::Quaterniond& q_ab_measured,
                         const Eigen::Vector3d& t_ab_measured,
                         const Eigen::Matrix<double, 6, 6>& sqrt_information)
        : q_ab_measured_(q_ab_measured), t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}

    template<typename T>
    bool operator()(const T* const p_a_ptr,
                    const T* const q_a_ptr,
                    const T* const p_b_ptr,
                    const T* const q_b_ptr,
                    T* residuals_ptr) const {
        Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
        Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);

        Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
        Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);

        // Compute the relative transformation between the two frames.
        Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
        Eigen::Quaternion<T> q_ab_estimated = q_b * q_a_inverse;

        // Represent the displacement between the two frames in the A frame.
        Eigen::Matrix<T, 3, 1> p_ab_estimated = -(q_b * q_a_inverse * p_a) + p_b;

        // Compute the error between the two orientation estimates.
        Eigen::Quaternion<T> delta_q = q_ab_measured_.template cast<T>() * q_ab_estimated.conjugate();

        // Compute the residuals.
        // [ position         ]   [ delta_p          ]
        // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
        Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
        residuals.template block<3, 1>(0, 0) = p_ab_estimated - t_ab_measured_.template cast<T>();
        residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();

        // Scale the residuals by the measurement uncertainty.
        residuals.applyOnTheLeft(sqrt_information_.template cast<T>());

        return true;
    }

    static ceres::CostFunction* Create(
        const Eigen::Quaterniond& q_ab_measured,
        const Eigen::Vector3d& t_ab_measured,
        const Eigen::Matrix<double, 6, 6>& sqrt_information) {
        return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
            new PoseGraph3dErrorTerm(q_ab_measured, t_ab_measured, sqrt_information));
    }

private:
    // The measurement for the position of B relative to A in the A frame.
    const Eigen::Quaterniond q_ab_measured_;
    const Eigen::Vector3d t_ab_measured_;
    // The square root of the measurement information matrix.
    const Eigen::Matrix<double, 6, 6> sqrt_information_;
};

class PoseGraph3dErrorTermWSq {
public:
    PoseGraph3dErrorTermWSq(const Eigen::Quaterniond& q_ab_measured,
                            const Eigen::Vector3d& t_ab_measured)
        : q_ab_measured_(q_ab_measured), t_ab_measured_(t_ab_measured) {}

    template<typename T>
    bool operator()(const T* const p_a_ptr,
                    const T* const q_a_ptr,
                    const T* const p_b_ptr,
                    const T* const q_b_ptr,
                    T* residuals_ptr) const {
        Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
        Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);

        Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
        Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);

        // Compute the relative transformation between the two frames.
        Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
        Eigen::Quaternion<T> q_ab_estimated = q_b * q_a_inverse;

        // Represent the displacement between the two frames in the A frame.
        Eigen::Matrix<T, 3, 1> p_ab_estimated = -(q_b * q_a_inverse * p_a) + p_b;

        // Compute the error between the two orientation estimates.
        Eigen::Quaternion<T> delta_q = q_ab_measured_.template cast<T>() * q_ab_estimated.conjugate();

        // Compute the residuals.
        // [ position         ]   [ delta_p          ]
        // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
        Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
        residuals.template block<3, 1>(0, 0) = p_ab_estimated - t_ab_measured_.template cast<T>();
        residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();

        return true;
    }

    static ceres::CostFunction* Create(
        const Eigen::Quaterniond& q_ab_measured,
        const Eigen::Vector3d& t_ab_measured) {
        return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTermWSq, 6, 3, 4, 3, 4>(
            new PoseGraph3dErrorTermWSq(q_ab_measured, t_ab_measured));
    }

private:
    // The measurement for the position of B relative to A in the A frame.
    const Eigen::Quaterniond q_ab_measured_;
    const Eigen::Vector3d t_ab_measured_;
};

inline Eigen::Matrix4d odomToMat(const Eigen::Quaterniond& q, const Eigen::Vector3d& t) {
    Eigen::Matrix4d T_glob_curr = Eigen::Matrix4d::Identity();
    T_glob_curr.block<3, 3>(0, 0) = q.toRotationMatrix();
    T_glob_curr.topRightCorner<3, 1>() = t;
    return T_glob_curr;
}

inline std::tuple<Eigen::Quaterniond, Eigen::Vector3d> matToQuat(const Eigen::Matrix4d& T) {
    Eigen::Quaterniond q(T.topLeftCorner<3, 3>());
    return std::make_tuple(q, T.topRightCorner<3, 1>());
}

template<typename Iterable>
Json::Value iterable2json(Iterable const& cont) {
    Json::Value v;
    for (auto&& element : cont) {
        v.append(element);
    }
    return v;
}

bool gps_ba::staticGPS_SLAM_Calibration(const openvslam::data::map_database* map_db, Eigen::Vector3d& gps_origin_lon_lat_alt) {
    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                    GeographicLib::Constants::WGS84_f());

    GeographicLib::LocalCartesian proj(gps_origin_lon_lat_alt.y(),
                                       gps_origin_lon_lat_alt.x(),
                                       gps_origin_lon_lat_alt.z(),
                                       earth);
    double alti = gps_origin_lon_lat_alt.z();

    auto kfs = map_db->get_all_keyframes();

    std::vector<Eigen::Vector3d> points_odom;
    points_odom.reserve(kfs.size());
    std::vector<Eigen::Vector3d> points_gps;
    points_gps.reserve(kfs.size());

    for (auto kf : kfs) {
        if (kf->gps_fix_.ts > 0) {
            if (gps_origin_lon_lat_alt.hasNaN()) {
                gps_origin_lon_lat_alt = kf->gps_fix_.lat_lon_alt;
                // convert from lat lon to lon lat
                std::swap(gps_origin_lon_lat_alt.x(), gps_origin_lon_lat_alt.y());
                // TODO ?? use zero or is it good ???
                alti = gps_origin_lon_lat_alt.z();
                proj.Reset(gps_origin_lon_lat_alt.y(),
                           gps_origin_lon_lat_alt.x(),
                           gps_origin_lon_lat_alt.z());
            }
            Eigen::Matrix4d T_curr_glob = kf->get_cam_pose();
            Eigen::Quaterniond q;
            Eigen::Vector3d t;
            std::tie(q, t) = matToQuat(T_curr_glob);
            points_odom.push_back(-(q.conjugate() * t));
            Eigen::Vector3d cart;
            proj.Forward(kf->gps_fix_.lat_lon_alt.x(), kf->gps_fix_.lat_lon_alt.y(), alti,
                         cart.x(), cart.y(), cart.z());
            points_gps.push_back(cart);
        }
    }

    if (points_gps.size() < 10)
        return false;

    // calculate transformation
    Eigen::Vector3d centroidA = Eigen::Vector3d::Zero();
    Eigen::Vector3d centroidB = Eigen::Vector3d::Zero();
    for (size_t i = 0; i < points_odom.size(); ++i) {
        centroidA += points_gps[i];
        centroidB += points_odom[i];
    }
    centroidA /= static_cast<double>(points_odom.size());
    centroidB /= static_cast<double>(points_odom.size());

    Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
    double sum_gps = 0;
    double sum_odom = 0;
    for (size_t i = 0; i < points_odom.size(); ++i) {
        Eigen::Vector3d P_gps_ = (points_gps[i] - centroidA);
        Eigen::Vector3d P_odom_ = (points_odom[i] - centroidB);
        sum_gps += P_gps_.squaredNorm();
        sum_odom += P_odom_.squaredNorm();
        H += P_gps_ * P_odom_.transpose();
    }

    double scale = std::sqrt(sum_odom / sum_gps);
    //double scale = 1.0;// std::sqrt(sum_odom / sum_gps);
    {
        std::ofstream depth_scale_file("depht_scale_factor.txt");
        depth_scale_file << std::setprecision(10);
        depth_scale_file << "Depth scale factor from depth to local ENU: " << 1.0 / scale << std::endl;
    }
    spdlog::info("Depth scale factor from depth to local ENU: {:03.2f}", 1.0 / scale);

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    svd.compute(H);

    Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose();

    // handling special case, reflection
    if (R.determinant() < 0) {
        Eigen::Matrix3d V = svd.matrixV();
        V.col(2) *= -1;
        R = V * svd.matrixU().transpose();
    }

    Eigen::Vector3d t = centroidB - scale * R * centroidA;

    Eigen::Matrix4d T_odom_gps = Eigen::Matrix4d::Identity();
    T_odom_gps.topLeftCorner<3, 3>() = R * scale;
    T_odom_gps.topRightCorner<3, 1>() = t;

    if (T_odom_gps.hasNaN() || !(R * R.transpose()).isIdentity(1e-4)) {
        return false;
    }

    Eigen::Matrix4d T_gps_odom = T_odom_gps.inverse();

    std::vector<Eigen::Matrix4d> T_curr_glob_old(kfs.size());
    size_t max_kf_num = map_db->get_max_keyframe_id() + 1;
    std::vector<size_t> kf_idxs(max_kf_num);
    size_t i = 0;
    for (auto kf : kfs) {
        kf_idxs[kf->id_] = i;
        T_curr_glob_old[i] = kf->get_cam_pose();
        Eigen::Matrix4d T_glob_curr = kf->get_cam_pose_inv();
        T_glob_curr.col(3) = T_gps_odom * T_glob_curr.col(3);
        T_glob_curr.block<3, 3>(0, 0) = R.transpose() * T_glob_curr.block<3, 3>(0, 0);
        // set new camera matrix
        kf->set_cam_pose(T_glob_curr.inverse());
        ++i;
    }

    // correct landmarks
    const auto lms = map_db->get_all_landmarks();
    std::vector<Eigen::Vector3d> landmarks_orig(lms.size());
    std::vector<Eigen::Vector3d> landmarks_transformed(lms.size());
    for (size_t i = 0; i < lms.size(); ++i) {
        landmarks_orig[i] = lms[i]->get_pos_in_world();
        if (landmarks_orig[i].norm() > 300)
            landmarks_orig[i] = Eigen::Vector3d::Zero();
        Eigen::Vector3d P = (T_gps_odom * lms[i]->get_pos_in_world().homogeneous()).head<3>();
        lms[i]->set_pos_in_world(P);
        landmarks_transformed[i] = lms[i]->get_pos_in_world();
        if (landmarks_transformed[i].norm() > 300)
            landmarks_transformed[i] = Eigen::Vector3d::Zero();
    }

    pointsToPly("landmarks_before.ply", landmarks_orig, 0, 255, 0);
    pointsToPly("landmarks_transformed.ply", landmarks_transformed, 0, 255, 0);
    // transformed gps
    std::vector<Eigen::Vector3d> transformed_gps;
    std::vector<Eigen::Vector3d> transformed_odom;
    transformed_gps.reserve(points_gps.size());
    transformed_odom.reserve(points_gps.size());
    for (size_t i = 0; i < points_odom.size(); ++i) {
        Eigen::Vector3d P = (T_odom_gps * points_gps[i].homogeneous()).head<3>();
        transformed_gps.push_back(P);
        P = (T_gps_odom * points_odom[i].homogeneous()).head<3>();
        transformed_odom.push_back(P);
    }

    pointsToPly("odom.ply", points_odom, 255, 0, 0);
    pointsToPly("gps.ply", points_gps, 0, 255, 0);
    pointsToPly("transformed_gps.ply", transformed_gps, 0, 0, 255);
    pointsToPly("transformed_odom.ply", transformed_odom, 0, 0, 255);
    return true;
}

void gps_ba::GPS_SLAM_loop_closure(const openvslam::data::map_database* map_db, Eigen::Vector3d& gps_origin_lon_lat_alt, double gps_p3d_loss) {
    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                    GeographicLib::Constants::WGS84_f());

    GeographicLib::LocalCartesian proj(gps_origin_lon_lat_alt.y(),
                                       gps_origin_lon_lat_alt.x(),
                                       gps_origin_lon_lat_alt.z(),
                                       earth);
    double alti = gps_origin_lon_lat_alt.z();

    auto kfs = map_db->get_all_keyframes();

    std::vector<Eigen::Vector3d> kf_poses(kfs.size());
    std::vector<Eigen::Quaterniond> kf_quats(kfs.size());
    size_t max_kf_idx = map_db->get_max_keyframe_id() + 1;
    std::vector<size_t> kf_ids(max_kf_idx);
    std::vector<Eigen::Vector3d> kf_poses_3d(kfs.size());
    std::vector<Eigen::Vector3d> gps_pts_tmp;
    std::vector<Eigen::Vector3d> odom_pts_tmp;
    std::vector<Eigen::Vector3i> color_tmp;

    ceres::Problem problem_odom_enu;
    for (size_t i = 0; i < kfs.size(); ++i) {
        kf_ids[kfs[i]->id_] = i;
        Eigen::Matrix4d T_curr_glob = kfs[i]->get_cam_pose();
        std::tie(kf_quats[i], kf_poses[i]) = matToQuat(T_curr_glob);
        kf_poses_3d[i] = -(kf_quats[i].conjugate() * kf_poses[i]);

        if (kfs[i]->gps_fix_.ts > 0) {
            Eigen::Vector3d P_gps;
            proj.Forward(kfs[i]->gps_fix_.lat_lon_alt.x(), kfs[i]->gps_fix_.lat_lon_alt.y(), /*kfs[i]->gps_fix_.lat_lon_alt.z()*/ alti,
                         //                         P_gps.z(), P_gps.x(), P_gps.y());
                         P_gps.x(), P_gps.y(), P_gps.z());
            //P_gps.y() *= -1;
            //P_gps.x() *= -1;
            //ceres::LossFunction* loss_function = nullptr;
            ceres::LossFunction* loss_function = new ceres::CauchyLoss(gps_p3d_loss);
            ceres::CostFunction* cost_function = PoseGraph3dPoseErrorTerm::Create(P_gps);
            problem_odom_enu.AddResidualBlock(cost_function,
                                              loss_function,
                                              kf_poses[i].data(),
                                              kf_quats[i].coeffs().data());
            gps_pts_tmp.push_back(P_gps);
            odom_pts_tmp.push_back(kf_poses_3d[i]);
            Eigen::Vector3i color = Eigen::Vector3i::Random();
            color.x() = color.x() % 255;
            color.y() = color.y() % 255;
            color.z() = color.z() % 255;
            color_tmp.push_back(color);
        }
    }
    pointsToPly("gps_pts_tmp.ply", gps_pts_tmp, color_tmp);
    pointsToPly("odom_pts_tmp.ply", odom_pts_tmp, color_tmp);
    pointsToPly("kf_poses_3d_before_gps_slam_loop_close.ply", kf_poses_3d, 0, 0, 255);

    Eigen::Matrix<double, 6, 6> sq = Eigen::Matrix<double, 6, 6>::Identity();
    for (size_t i = 0; i < kfs.size(); ++i) {
        auto childrens = kfs[i]->graph_node_->get_spanning_children();
        Eigen::Matrix4d T_prev_glob = kfs[i]->get_cam_pose();
        for (auto& c : childrens) {
            size_t child_idx = kf_ids[c->id_];
            Eigen::Matrix4d T_curr_glob = c->get_cam_pose();

            Eigen::Matrix4d T_curr_prev = T_curr_glob * T_prev_glob.inverse();
            Eigen::Quaterniond q;
            Eigen::Vector3d p;
            std::tie(q, p) = matToQuat(T_curr_prev);

            ceres::LossFunction* loss_function = nullptr;
            //ceres::LossFunction* loss_function = new ceres::CauchyLoss(gps_p_loss);
            ceres::CostFunction* cost_function = PoseGraph3dErrorTerm::Create(q, p, sq);

            problem_odom_enu.AddResidualBlock(cost_function,
                                              loss_function,
                                              kf_poses[i].data(),
                                              kf_quats[i].coeffs().data(),
                                              kf_poses[child_idx].data(),
                                              kf_quats[child_idx].coeffs().data());
        }
    }

    ceres::LocalParameterization* quaternion_local_parameterization = new ceres::EigenQuaternionParameterization();

    for (size_t i = 0; i < kfs.size(); ++i) {
        if (problem_odom_enu.HasParameterBlock(kf_quats[i].coeffs().data())) {
            problem_odom_enu.SetParameterization(kf_quats[i].coeffs().data(),
                                                 quaternion_local_parameterization);
        }
    }

    //std::vector<Eigen::Vector3d> kf_poses_o = kf_poses;
    //std::vector<Eigen::Quaterniond> kf_quats_o = kf_quats;

    // solve problem once
    {
        ceres::Solver::Options options;
        options.max_num_iterations = 100;
        options.num_threads = 12;
        options.linear_solver_type = ceres::SPARSE_SCHUR;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem_odom_enu, &summary);
        spdlog::info(summary.FullReport());
    }

 //std::vector<Eigen::Vector3d> kf_poses_3d(kf_poses.size());
    for (size_t i = 0; i < kf_poses.size(); ++i) {
        if (kfs[i] != nullptr) {
            Eigen::Matrix4d T_curr_glob_new = odomToMat(kf_quats[i], kf_poses[i]);
            Eigen::Matrix4d T_glob_curr_new = T_curr_glob_new.inverse();
            Eigen::Matrix4d T_curr_glob = kfs[i]->get_cam_pose();
            // correct landmark positions and kf pose
            auto landmarks = kfs[i]->get_landmarks();
            for (auto& l : landmarks) {
                if (l != nullptr && l->get_ref_keyframe()->id_ == kfs[i]->id_) {
                    Eigen::Vector3d P = l->get_pos_in_world();
                    P = (T_glob_curr_new * T_curr_glob * P.homogeneous()).head<3>();
                    l->set_pos_in_world(P);
                }
            }
            kfs[i]->set_cam_pose(T_curr_glob_new);
        }
        kf_poses_3d[i] = -(kf_quats[i].conjugate() * kf_poses[i]);
    }

    //for (size_t i = 0; i < kfs.size(); ++i) {
    //    kfs[i]->set_cam_pose(odomToMat(kf_quats[i], kf_poses[i]));
    //    kf_poses_3d[i] = -(kf_quats[i].conjugate() * kf_poses[i]);
    //}
    pointsToPly("kf_poses_3d_after_gps_slam_loop_close.ply", kf_poses_3d, 0, 0, 255);
}

bool gps_ba::relocalization_correct_map_poses(openvslam::data::frame& curr_frm, const Eigen::Matrix4d T_curr_glob_old, uint32_t corrected_map_kf_max_id, uint32_t num_localized_landmark_thrs) {
    auto kf_parent = curr_frm.ref_keyfrm_->graph_node_->get_spanning_parent();
    auto cfid = curr_frm.id_;
    Eigen::Matrix4d T_curr_glob = T_curr_glob_old;
    std::vector<Eigen::Vector3d> kf_poses;
    kf_poses.reserve(1000);
    std::vector<Eigen::Quaterniond> kf_quats;
    kf_quats.reserve(1000);
    Eigen::Quaterniond q;
    Eigen::Vector3d p;
    std::tie(q, p) = matToQuat(curr_frm.cam_pose_cw_);
    kf_poses.push_back(p);
    kf_quats.push_back(q);
    ceres::Problem problem_lc;
    std::vector<openvslam::data::keyframe*> kfs;
    kfs.reserve(1000);
    kfs.push_back(nullptr);
    size_t f_num = 0;
    while (kf_parent != nullptr) {
        if (kf_parent->id_ >= corrected_map_kf_max_id && kf_parent->num_localized_landmark_ < num_localized_landmark_thrs
            && f_num < 980) {
            kfs.push_back(kf_parent);
            cfid = kf_parent->id_;

            Eigen::Matrix4d T_prev_glob = kf_parent->get_cam_pose();
            std::tie(q, p) = matToQuat(T_prev_glob);
            kf_poses.push_back(p);
            kf_quats.push_back(q);
            Eigen::Matrix4d T_curr_prev = T_curr_glob * T_prev_glob.inverse();
            std::tie(q, p) = matToQuat(T_curr_prev);

            ceres::LossFunction* loss_function = nullptr;
            //ceres::LossFunction* loss_function = new ceres::CauchyLoss(gps_p_loss);
            ceres::CostFunction* cost_function = PoseGraph3dErrorTermWSq::Create(q, p);

            problem_lc.AddResidualBlock(cost_function,
                                        loss_function,
                                        kf_poses[f_num + 1].data(),
                                        kf_quats[f_num + 1].coeffs().data(),
                                        kf_poses[f_num].data(),
                                        kf_quats[f_num].coeffs().data());
            T_curr_glob = T_prev_glob;
            kf_parent = kf_parent->graph_node_->get_spanning_parent();
            // increment keyframe counter
            ++f_num;
        }
        else {
            //++i;
            break;
        }
    }

    if (kf_parent != nullptr) {
        Eigen::Matrix4d T_prev_glob = kf_parent->get_cam_pose();
        std::tie(q, p) = matToQuat(T_prev_glob);
        kf_poses.push_back(p);
        kf_quats.push_back(q);
        kfs.push_back(kf_parent);
        Eigen::Matrix4d T_curr_prev = T_curr_glob * T_prev_glob.inverse();
        std::tie(q, p) = matToQuat(T_curr_prev);

        ceres::LossFunction* loss_function = nullptr;
        //ceres::LossFunction* loss_function = new ceres::CauchyLoss(gps_p_loss);
        ceres::CostFunction* cost_function = PoseGraph3dErrorTermWSq::Create(q, p);

        problem_lc.AddResidualBlock(cost_function,
                                    loss_function,
                                    kf_poses[f_num + 1].data(),
                                    kf_quats[f_num + 1].coeffs().data(),
                                    kf_poses[f_num].data(),
                                    kf_quats[f_num].coeffs().data());
    }

    if (kf_poses.size() > 2) {
        ceres::LocalParameterization* quaternion_local_parameterization = new ceres::EigenQuaternionParameterization();

        for (size_t i = 0; i < kf_quats.size(); ++i) {
            //if (problem_lc.HasParameterBlock(kf_quats[i].coeffs().data())) {
            problem_lc.SetParameterization(kf_quats[i].coeffs().data(),
                                           quaternion_local_parameterization);
            //}
        }
        problem_lc.SetParameterBlockConstant(kf_poses.front().data());
        problem_lc.SetParameterBlockConstant(kf_poses.back().data());
        problem_lc.SetParameterBlockConstant(kf_quats.front().coeffs().data());
        problem_lc.SetParameterBlockConstant(kf_quats.back().coeffs().data());

        // solve problem once
        {
            ceres::Solver::Options options;
            options.max_num_iterations = 100;
            options.num_threads = 12;
            options.linear_solver_type = ceres::SPARSE_SCHUR;
            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem_lc, &summary);
            spdlog::info("Relocalization keyframe correction done {}", summary.BriefReport());
        }
        //std::vector<Eigen::Vector3d> kf_poses_3d(kf_poses.size());
        for (size_t i = 0; i < kf_poses.size(); ++i) {
            if (kfs[i] != nullptr) {
                Eigen::Matrix4d T_curr_glob_new = odomToMat(kf_quats[i], kf_poses[i]);
                Eigen::Matrix4d T_glob_curr_new = T_curr_glob_new.inverse();
                Eigen::Matrix4d T_curr_glob = kfs[i]->get_cam_pose();
                // correct landmark positions and kf pose
                auto landmarks = kfs[i]->get_landmarks();
                for (auto& l : landmarks) {
                    if (l != nullptr && l->get_ref_keyframe()->id_ == kfs[i]->id_) {
                        Eigen::Vector3d P = l->get_pos_in_world();
                        P = (T_glob_curr_new * T_curr_glob * P.homogeneous()).head<3>();
                        l->set_pos_in_world(P);
                    }
                }
                kfs[i]->set_cam_pose(T_curr_glob_new);
            }
            //kf_poses_3d[i] = -(kf_quats[i].conjugate() * kf_poses[i]);
        }
        //pointsToPly("corrected_localization_ptr.ply", kf_poses_3d, 0, 0, 255);
    }
    else {
        return false;
    }
    return true;
}

void gps_ba::global_BA_GPS_SLAM(const openvslam::data::map_database* map_db, const openvslam::camera::base* camera, const Eigen::Vector3d& gps_origin_lon_lat_alt, double gps_p3d_loss_ba, double landmark_reproj_loss) {
    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                    GeographicLib::Constants::WGS84_f());

    GeographicLib::LocalCartesian proj(gps_origin_lon_lat_alt.y(),
                                       gps_origin_lon_lat_alt.x(),
                                       gps_origin_lon_lat_alt.z(),
                                       earth);
    double alti = gps_origin_lon_lat_alt.z();

    auto kfs = map_db->get_all_keyframes();
    // bundle adjustment after odom enum calibration
    // ---------------------------------------------
    // ---------------------------------------------
    //----------------------------------------------
    ceres::Problem problem_ba;
    std::vector<Eigen::Vector3d> kf_poses(kfs.size());
    std::vector<Eigen::Quaterniond> kf_quats(kfs.size());
    size_t max_kf_idx = map_db->get_max_keyframe_id() + 1;
    std::vector<size_t> kf_ids(max_kf_idx);

    std::vector<Eigen::Vector3d> kf_poses_3d(kfs.size());

    for (size_t i = 0; i < kfs.size(); ++i) {
        kf_ids[kfs[i]->id_] = i;
        Eigen::Matrix4d T_curr_glob = kfs[i]->get_cam_pose();
        std::tie(kf_quats[i], kf_poses[i]) = matToQuat(T_curr_glob);
        kf_poses_3d[i] = -(kf_quats[i].conjugate() * kf_poses[i]);
        if (kfs[i]->gps_fix_.ts > 0) {
            Eigen::Vector3d P_gps;
            proj.Forward(kfs[i]->gps_fix_.lat_lon_alt.x(), kfs[i]->gps_fix_.lat_lon_alt.y(), /*kfs[i]->gps_fix_.lat_lon_alt.z()*/ alti,
                         //                         P_gps.z(), P_gps.x(), P_gps.y());
                         P_gps.x(), P_gps.y(), P_gps.z());
            //P_gps.y() *= -1;
            //P_gps.x() *= -1;
            //ceres::LossFunction* loss_function = nullptr;
            ceres::LossFunction* loss_function = new ceres::CauchyLoss(gps_p3d_loss_ba);
            ceres::CostFunction* cost_function = PoseGraph3dPoseErrorTerm::Create(P_gps);
            problem_ba.AddResidualBlock(cost_function,
                                        loss_function,
                                        kf_poses[i].data(),
                                        kf_quats[i].coeffs().data());
        }
    }
    pointsToPly("odom_before_ba.ply", kf_poses_3d, 255, 0, 0);

    // add point correspondences constraint to bundle adjustment
    const auto lms = map_db->get_all_landmarks();
    std::vector<Eigen::Vector3d> l_ps(lms.size());
    size_t num_g_lm = 0;
    const openvslam::camera::perspective* p_cam = dynamic_cast<const openvslam::camera::perspective*>(camera);
    double fx = p_cam->fx_;
    double fy = p_cam->fy_;
    double cx = p_cam->cx_;
    double cy = p_cam->cy_;
    for (size_t i = 0; i < lms.size(); ++i) {
        size_t ref_kf_id = kf_ids[lms[i]->get_ref_keyframe()->id_];
        l_ps[i] = lms[i]->get_pos_in_world();
        auto obs = lms[i]->get_observations();
        for (auto& o : obs) {
            auto kp = o.first->keypts_[o.second];
            auto kf_idx = kf_ids[o.first->id_];
            Eigen::Vector3d P_cam = kf_quats[kf_idx] * l_ps[i] + kf_poses[kf_idx];
            P_cam /= P_cam.z();
            double u = P_cam.x() * fx + cx - kp.pt.x;
            double v = P_cam.y() * fy + cy - kp.pt.y;
            double err = u * u + v * v;
            //if (err < 2.0f * 2.0f)
            {
                ++num_g_lm;
                //ceres::LossFunction* loss_function = nullptr;
                ceres::LossFunction* loss_function = new ceres::CauchyLoss(landmark_reproj_loss);
                ceres::CostFunction* cost_function = SnavelyReprojectionError::Create(kp.pt.x, kp.pt.y,
                                                                                fx, fy, cx, cy);
                problem_ba.AddResidualBlock(cost_function,
                                            loss_function,
                                            kf_quats[kf_idx].coeffs().data(),
                                            kf_poses[kf_idx].data(),
                                            l_ps[i].data());
            }
        }
    }

    ceres::LocalParameterization* quaternion_local_parameterization2 = new ceres::EigenQuaternionParameterization();

    size_t min_kfs_id = 210000000;
    size_t max_kfs_id = 0;
    for (size_t i = 0; i < kfs.size(); ++i) {
        if (problem_ba.HasParameterBlock(kf_quats[i].coeffs().data())) {
            problem_ba.SetParameterization(kf_quats[i].coeffs().data(),
                                           quaternion_local_parameterization2);
        }
        if (min_kfs_id > kfs[i]->id_) {
            min_kfs_id = kfs[i]->id_;
        }
        if (max_kfs_id < kfs[i]->id_) {
            max_kfs_id = kfs[i]->id_;
        }
    }

    //problem_ba.SetParameterBlockConstant(kf_poses[kf_ids[min_kfs_id]].data());
    //problem_ba.SetParameterBlockConstant(kf_poses[kf_ids[max_kfs_id]].data());

    ceres::Solver::Options options;
    options.max_num_iterations = 20;
    options.linear_solver_type = ceres::SPARSE_SCHUR;
    options.num_threads = 12;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem_ba, &summary);
    spdlog::info(summary.FullReport());

    for (size_t i = 0; i < lms.size(); ++i) {
        lms[i]->set_pos_in_world(l_ps[i]);
        if (l_ps[i].norm() > 300)
            l_ps[i] = Eigen::Vector3d::Zero();
    }
    for (size_t i = 0; i < kfs.size(); ++i) {
        kfs[i]->set_cam_pose(odomToMat(kf_quats[i], kf_poses[i]));
        kf_poses_3d[i] = -(kf_quats[i].conjugate() * kf_poses[i]);
    }

    pointsToPly("odom_after_ba.ply", kf_poses_3d, 0, 255, 0);
    pointsToPly("landmarks_after_ba.ply", l_ps, 0, 255, 0);
}

void gps_ba::saveGeoJson(const openvslam::data::map_database* map_db, const openvslam::camera::base* camera, const Eigen::Vector3d& gps_origin_lon_lat_alt) {
    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
                                    GeographicLib::Constants::WGS84_f());

    GeographicLib::LocalCartesian proj(gps_origin_lon_lat_alt.y(),
                                       gps_origin_lon_lat_alt.x(),
                                       gps_origin_lon_lat_alt.z(),
                                       earth);
    Json::Value root;
    root["type"] = "FeatureCollection";
    Json::Value features;
    Json::Value origin;
    origin["type"] = "Feature";
    Json::Value geometry;
    geometry["type"] = "Point";
    std::vector<double> g_orig = {gps_origin_lon_lat_alt.x(), gps_origin_lon_lat_alt.y()};
    geometry["coordinates"] = iterable2json(g_orig);
    origin["geometry"] = geometry;
    origin["properties"]["object_type"] = "origin";
    features.append(origin);
    //{
    Json::Value trajectory;
    trajectory["type"] = "Feature";
    trajectory["properties"]["traffic_info"]["backward"] = "forbidden";
    Json::Value geometry2;
    geometry2["type"] = "LineString";

    const auto kfs = map_db->get_all_keyframes();
    size_t max_kf_idx = map_db->get_max_keyframe_id() + 1;
    std::vector<size_t> kf_ids(max_kf_idx, size_t(-1));

    double p3d_loss = 0.01;
    double lm_loss = 1.0;
    std::vector<Eigen::Vector3d> kf_poses_3d(kfs.size());

    for (size_t i = 0; i < kfs.size(); ++i) {
        kf_ids[kfs[i]->id_] = i;
    }
    Json::Value coordinates;
    for (size_t i = 0; i < kf_ids.size(); ++i) {
        if (kf_ids[i] != size_t(-1)) {
            std::vector<double> lon_lat(2);
            double alt;
            Eigen::Vector3d P = kfs[kf_ids[i]]->get_cam_pose_inv().col(3).head<3>();
            proj.Reverse(P.x(), P.y(), P.z(), lon_lat[1], lon_lat[0], alt);
            coordinates.append(iterable2json(lon_lat));
        }
    }
    geometry2["coordinates"] = coordinates;
    trajectory["geometry"] = geometry2;
    features.append(trajectory);
    //}
    root["features"] = features;
    std::ofstream file_id("geojson.json");
    Json::StyledStreamWriter writer;
    writer.write(file_id, root);
}


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

using namespace Eigen;

int main()
{
  Matrix2d a;
  a << 1, 2,
       3, 4;
  Vector3d v(1,2,3);
  std::cout << "a * 2.5 =\n" << a * 2.5 << std::endl;
  std::cout << "0.1 * v =\n" << 0.1 * v << std::endl;
  std::cout << "Doing v *= 2;" << std::endl;
  v *= 2;
  std::cout << "Now v =\n" << v << std::endl;
}


#include <boost/property_tree/ini_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <complex>
#include <cstddef>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <istream>
#include <vector>

using std::complex;
using std::cout;
using std::int8_t;
using std::vector;

namespace pt = boost::property_tree;
struct Config {
  double cxmin;
  double cxmax;
  double cymin;
  double cymax;
  size_t max_iterations;
  size_t ixsize;
  size_t iysize;
  friend std::ostream &operator<<(std::ostream &stream, const Config &config) {
    return stream << "Config \n"
                  << "{ cxmin: " << config.cxmin << " ,cxmax: " << config.cxmax
                  << "}\n"
                  << "{ cymin: " << config.cymin << " ,cymax: " << config.cymax
                  << "}\n"
                  << " Number of iterations: " << config.max_iterations << "\n"
                  << "{ ixsize: " << config.iysize
                  << " ,iysize: " << config.ixsize << "}\n";
  }
};

class Mandelbrot {
  Config config;
  vector<vector<int8_t>> image;

public:
  Mandelbrot(Config config)
      : config(config), image(config.iysize, vector<int8_t>(config.ixsize, 0)) {
#ifdef DEBUG
    cout << config;
#endif
  }

  void compute() {
    double cxmin = config.cxmin;
    double cymin = config.cymin;
    double cxmax = config.cxmax;
    double cymax = config.cymax;

    const int width = config.iysize;
    const int height = config.ixsize;
    const int num_pixels = width * height;

    // const complex<double> center((cxmax - cxmin) / 2.0, (cymax - cymin) / 2.0);
    const complex<double> begin = complex<double>(cxmin, cymin);
    double xinc = (cxmax - cxmin) / width;
    double yinc = (cymax - cymin) / height;

#pragma omp parallel for schedule(dynamic)
    for (int pix = 0; pix < num_pixels; ++pix) {
      const int x = pix % width, y = pix / width;

      complex<double> c = begin + complex<double>(y*yinc, x * xinc);
      // cout <<  c << " " << x << " " << y << "\n" ;

      complex<double> z = c;
      size_t iteration = 0;
      for (; iteration < config.max_iterations; ++iteration) {
        if (std::abs(z) >= 2)
          break;
        z = z * z + c;
      }
      // cout << x << " " << y << " " << iteration << "\n";
      if (iteration == config.max_iterations)
        iteration = 4;

      {
        // TODO: Fix coloring
        if (iteration != 4) {
          image[x][y] = iteration % 4;
        }
      }
    }
  }

  void serialize() {
    for (auto row : image) {
      for (auto pixel : row) {
        cout << (pixel ? "*" : " ");
      }
      cout << "\n";
    }
  }

  // Loosely based on https://github.com/skeeto/mandel-simd/blob/master/mandel.c
  // TODO: Fix coloring
  void write_image() {
    uint8_t imgbuf[3 * config.iysize * config.ixsize];
    const uint32_t color[] = {0xf53d3d, 0xf53d3d, 0xf53d93, 0xf53d93, 0xffffff};
    const size_t width = config.iysize;
    const int height = config.ixsize;
    const int num_pixels = width * height;
    for (int pix = 0; pix < num_pixels; ++pix) {
      const int x = pix % width, y = pix / width;
        // printf("%zu %zu %d\n", x, y, image[x][y]);
        uint8_t pxlclr = color[image[x][y]];
        // cout << y * 3 * config.ixsize + 3 * x << "\n";
        imgbuf[x * 3 * config.ixsize + 3 * y + 0] = pxlclr >> 16;
        imgbuf[x * 3 * config.ixsize + 3 * y + 1] = pxlclr >> 8;
        imgbuf[x * 3 * config.ixsize + 3 * y + 2] = pxlclr >> 0;
    }
    fprintf(stdout, "P6\n%zu %zu\n%d\n", config.ixsize, config.iysize, 255);
    fwrite(imgbuf, sizeof(imgbuf), 1, stdout);
  }
};

int main(int argc, char **argv) {
  pt::ptree tree;
  pt::read_ini("./config.ini", tree);

  Mandelbrot mandel({
      .cxmin = tree.get<double>("xmin", -2.5),
      .cxmax = tree.get<double>("xmax", 1.5),

      .cymin = tree.get<double>("ymin", -2.5),
      .cymax = tree.get<double>("ymax", 1.5),

      .max_iterations = tree.get<size_t>("max_iterations", 256),

      .ixsize = tree.get<size_t>("ixsize", 1000),
      .iysize = tree.get<size_t>("iysize", 1000),
  });
  mandel.compute();
#ifdef DEBUG
  mandel.serialize();
#endif
  mandel.write_image();

  return 0;
}