// PGASGraph
#include <pgas-graph/graph-utilities.hpp>
#include <pgas-graph/pgas-graph.h>

// STL
#include <chrono>
#include <iostream>
#include <random>

// Boost
#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

int main() {
	using namespace boost;

	using Graph = adjacency_list<vecS, vecS, undirectedS,
			 property<vertex_distance_t, int>,
			 property<edge_weight_t, int>>;

	using E = std::pair<int, int>;

	const int num_nodes = 1024;
	const int percentage = 10;

	Graph g{GraphUtilities::generateRandomConnectedGraph<Graph>(
			num_nodes, percentage)};

	// Assign weights to the edges.
	auto edges = boost::edges(g);
	int weight{0};
	// for (auto it = edges.first; it != edges.second; ++it) {
	// 	// g[*it] = weight++;
	// }

	std::vector<graph_traits<Graph>::vertex_descriptor> p(num_vertices(g));
	prim_minimum_spanning_tree(g, &p[0]);
	for (std::size_t i = 0; i != p.size(); ++i) {
		if (p[i] != i) {
			std::cout << "parent[" << i << "] = " << p[i] << std::endl;
		} else {
			std::cout << "parent[" << i << "] = no parent" << std::endl;
		}
	}

	return EXIT_SUCCESS;
}


#ifdef STAN_OPENCL
#include <stan/math/prim/mat.hpp>
#include <stan/math/opencl/copy.hpp>
#include <stan/math/opencl/multiply.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>
#include <algorithm>
boost::random::mt19937 rng;

#define EXPECT_MATRIX_NEAR(A, B, DELTA) \
  for (int i = 0; i < A.size(); i++)    \
    EXPECT_NEAR(A(i), B(i), DELTA);

TEST(MathMatrix, vector_row_vector) {
  stan::math::vector_d v(3);
  stan::math::row_vector_d rv(3);
  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m_cl(1, 1);
  EXPECT_NO_THROW(m_cl = rv_cl * v_cl);
}

TEST(MathMatrix, one_dim_zero_matrix) {
  stan::math::matrix_d m0(5, 0);
  stan::math::matrix_d m1(0, 3);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_NO_THROW(m0_cl * m1_cl);

  EXPECT_NO_THROW(m0_cl * 2.0);
  EXPECT_NO_THROW(2.0 * m0_cl);

  EXPECT_NO_THROW(m1_cl * 2.0);
  EXPECT_NO_THROW(2.0 * m1_cl);
}

TEST(MathMatrix, zero_result_matrix) {
  stan::math::matrix_d m0(0, 5);
  stan::math::matrix_d m1(5, 0);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_NO_THROW(m0_cl * m1_cl);
}

TEST(MathMatrix, zero_size_input_matrix) {
  stan::math::matrix_d m0(0, 0);
  stan::math::matrix_d m1(0, 0);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_NO_THROW(m0_cl * m1_cl);

  EXPECT_NO_THROW(m0_cl * 2.0);
  EXPECT_NO_THROW(2.0 * m0_cl);
}

TEST(MathMatrix, non_matching_dim_excpetion) {
  stan::math::matrix_d m0(5, 3);
  stan::math::matrix_d m1(2, 6);

  stan::math::matrix_cl<double> m0_cl(m0);
  stan::math::matrix_cl<double> m1_cl(m1);
  EXPECT_THROW(m0_cl * m1_cl, std::invalid_argument);
}

TEST(MathMatrix, multiply_scalar) {
  auto v = stan::math::vector_d::Random(25).eval();
  stan::math::vector_d v_cl_res(25);
  auto rv = stan::math::row_vector_d::Random(25).eval();
  stan::math::row_vector_d rv_cl_res(25);
  auto m = stan::math::matrix_d::Random(5, 5).eval();
  stan::math::matrix_d m_cl_res(5, 5);

  stan::math::matrix_cl<double> v_cl(v);
  v_cl = v_cl * 2.0;
  v_cl_res = stan::math::from_matrix_cl(v_cl);

  stan::math::matrix_cl<double> rv_cl(rv);
  rv_cl = rv_cl * 2.0;
  rv_cl_res = stan::math::from_matrix_cl(rv_cl);

  stan::math::matrix_cl<double> m_cl(m);
  m_cl = m_cl * 2.0;
  m_cl_res = stan::math::from_matrix_cl(m_cl);

  v = v * 2.0;
  rv = rv * 2.0;
  m = m * 2.0;

  EXPECT_MATRIX_NEAR(v, v_cl_res, 1e-10);
  EXPECT_MATRIX_NEAR(rv, rv_cl_res, 1e-10);
  EXPECT_MATRIX_NEAR(m, m_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_vector) {
  auto v = stan::math::vector_d::Random(5).eval();
  auto rv = stan::math::row_vector_d::Random(5).eval();
  stan::math::matrix_d m0(1, 1);
  stan::math::matrix_d m0_cl_res(1, 1);
  stan::math::matrix_d m1(5, 5);
  stan::math::matrix_d m1_cl_res(5, 5);

  m0 = rv * v;
  m1 = v * rv;

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 1);
  stan::math::matrix_cl<double> m1_cl(5, 5);

  m0_cl = rv_cl * v_cl;
  m1_cl = v_cl * rv_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  m1_cl_res = stan::math::from_matrix_cl(m1_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
  EXPECT_MATRIX_NEAR(m1, m1_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_small) {
  auto m = stan::math::matrix_d::Random(4, 5).eval();
  auto v = stan::math::vector_d::Random(5).eval();
  stan::math::matrix_d m0(4, 1);
  stan::math::matrix_d m0_cl_res(4, 1);

  m0 = m * v;

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(4, 1);

  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_big) {
  auto m = stan::math::matrix_d::Random(400, 600).eval();
  auto v = stan::math::vector_d::Random(600).eval();
  stan::math::matrix_d m0(400, 1);
  stan::math::matrix_d m0_cl_res(400, 1);

  m0 = m * v;

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(400, 1);

  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_small) {
  auto m = stan::math::matrix_d::Random(5, 4).eval();
  auto rv = stan::math::row_vector_d::Random(5).eval();
  stan::math::matrix_d m0(1, 4);
  stan::math::matrix_d m0_cl_res(1, 4);

  m0 = rv * m;

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 4);

  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_big) {
  auto m = stan::math::matrix_d::Random(600, 400).eval();
  auto rv = stan::math::row_vector_d::Random(600).eval();
  stan::math::matrix_d m0(1, 400);
  stan::math::matrix_d m0_cl_res(1, 400);

  m0 = rv * m;

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 400);

  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);

  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_tri_small) {
  auto m = stan::math::matrix_d::Random(4, 5).eval();
  auto v = stan::math::vector_d::Random(5).eval();
  stan::math::matrix_d m0(4, 1);
  stan::math::matrix_d m0_cl_res(4, 1);

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(4, 1);

  m0 = m * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Lower>() * v;
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Upper>() * v;
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, matrix_vector_tri_big) {
  auto m = stan::math::matrix_d::Random(400, 600).eval();
  auto v = stan::math::vector_d::Random(600).eval();
  stan::math::matrix_d m0(400, 1);
  stan::math::matrix_d m0_cl_res(400, 1);

  stan::math::matrix_cl<double> v_cl(v);
  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> m0_cl(400, 1);

  m0 = m * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Lower>() * v;
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Entire);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = m.triangularView<Eigen::Upper>() * v;
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Lower>())
       * v.triangularView<Eigen::Upper>();
  m_cl.view(stan::math::matrix_cl_view::Lower);
  v_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(m.triangularView<Eigen::Upper>())
       * v.triangularView<Eigen::Lower>();
  m_cl.view(stan::math::matrix_cl_view::Upper);
  v_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = m_cl * v_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_tri_small) {
  auto m = stan::math::matrix_d::Random(5, 4).eval();
  auto rv = stan::math::row_vector_d::Random(5).eval();
  stan::math::matrix_d m0(1, 4);
  stan::math::matrix_d m0_cl_res(1, 4);

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 4);

  m0 = rv * m;
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Lower>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Upper>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, row_vector_matrix_tri_big) {
  auto m = stan::math::matrix_d::Random(600, 400).eval();
  auto rv = stan::math::row_vector_d::Random(600).eval();
  stan::math::matrix_d m0(1, 400);
  stan::math::matrix_d m0_cl_res(1, 400);

  stan::math::matrix_cl<double> m_cl(m);
  stan::math::matrix_cl<double> rv_cl(rv);
  stan::math::matrix_cl<double> m0_cl(1, 400);

  m0 = rv * m;
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Lower>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Entire);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = rv.triangularView<Eigen::Upper>() * m;
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Entire);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  // the cast is needed because operator* for for two triangular views does not
  // exist
  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Lower>())
       * m.triangularView<Eigen::Upper>();
  rv_cl.view(stan::math::matrix_cl_view::Lower);
  m_cl.view(stan::math::matrix_cl_view::Upper);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);

  m0 = static_cast<Eigen::MatrixXd>(rv.triangularView<Eigen::Upper>())
       * m.triangularView<Eigen::Lower>();
  rv_cl.view(stan::math::matrix_cl_view::Upper);
  m_cl.view(stan::math::matrix_cl_view::Lower);
  m0_cl = rv_cl * m_cl;
  m0_cl_res = stan::math::from_matrix_cl(m0_cl);
  EXPECT_MATRIX_NEAR(m0, m0_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_small) {
  using stan::math::multiply;
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_big) {
  using stan::math::multiply;
  int size = 512;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, lower_tri_rect_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Lower>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, lower_tri_rect_multiply_big) {
  int size = 512;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Lower>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, lower_tri_rect_multiply_big_rect) {
  int size = 321;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size * 3).eval();
  stan::math::matrix_d m3_cl_res(size, size * 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Lower>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, upper_tri_rect_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Upper);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Upper>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, upper_tri_rect_multiply_big) {
  int size = 472;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Upper);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Upper>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, upper_tri_rect_multiply_big_rect) {
  int size = 463;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size * 3).eval();
  stan::math::matrix_d m3_cl_res(size, size * 3);

  stan::math::matrix_cl<double> m11(m1, stan::math::matrix_cl_view::Upper);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1.triangularView<Eigen::Upper>() * m2).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_lower_tri_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Lower);

  auto m3 = (m1 * m2.triangularView<Eigen::Lower>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_lower_tri_multiply_big) {
  int size = 451;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Lower);

  auto m3 = (m1 * m2.triangularView<Eigen::Lower>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_lower_tri_multiply_big_rect) {
  int size = 444;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size * 3).eval();
  stan::math::matrix_d m3_cl_res(size, size * 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Lower);

  auto m3 = (m1 * m2.triangularView<Eigen::Lower>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_upper_tri_multiply_small) {
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  auto m2 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m3_cl_res(3, 3);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Upper);

  auto m3 = (m1 * m2.triangularView<Eigen::Upper>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_upper_tri_multiply_big) {
  int size = 468;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Upper);

  auto m3 = (m1 * m2.triangularView<Eigen::Upper>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, rect_upper_tri_multiply_big_rect) {
  int size = 345;
  auto m1 = stan::math::matrix_d::Random(size * 3, size).eval();
  auto m2 = stan::math::matrix_d::Random(size, size).eval();
  stan::math::matrix_d m3_cl_res(size * 3, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2, stan::math::matrix_cl_view::Upper);

  auto m3 = (m1 * m2.triangularView<Eigen::Upper>()).eval();

  auto m33 = m11 * m22;

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_big_split_4) {
  using stan::math::multiply;
  int size = 512;
  auto m1 = stan::math::matrix_d::Random(size, size * 2).eval();
  auto m2 = stan::math::matrix_d::Random(size * 2, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = stan::math::opencl::multiply(m11, m22);

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_big_split_11) {
  using stan::math::multiply;
  int size = 433;
  auto m1 = stan::math::matrix_d::Random(size, size * 11).eval();
  auto m2 = stan::math::matrix_d::Random(size * 11, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = stan::math::opencl::multiply(m11, m22);

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}

TEST(MathMatrix, multiply_small_split_big) {
  using stan::math::multiply;
  int size = 32;
  auto m1 = stan::math::matrix_d::Random(size, size * 10).eval();
  auto m2 = stan::math::matrix_d::Random(size * 10, size).eval();
  stan::math::matrix_d m3_cl_res(size, size);

  stan::math::matrix_cl<double> m11(m1);
  stan::math::matrix_cl<double> m22(m2);

  auto m3 = (m1 * m2).eval();

  auto m33 = stan::math::opencl::multiply(m11, m22);

  m3_cl_res = stan::math::from_matrix_cl(m33);

  EXPECT_MATRIX_NEAR(m3, m3_cl_res, 1e-10);
}
#endif


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#ifndef BOGUS_EIGEN_SERIALIZATION_HPP
#define BOGUS_EIGEN_SERIALIZATION_HPP

#include <Eigen/Core>

#ifndef BOGUS_BLOCK_WITHOUT_EIGEN_SPARSE
#include "SparseHeader.hpp"
#endif

namespace boost
{
namespace serialization
{

template<typename Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
inline void load(
	   Archive & ar,
	   Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
inline void save(
	   Archive & ar,
	   const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Cols, int _Options, int _MaxRows, int _MaxCols>
inline void load(
	   Archive & ar,
	   Eigen::Matrix<_Scalar, Eigen::Dynamic, _Cols, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	int rows ;
	ar & rows ;
	matrix.resize( rows, _Cols ) ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Cols, int _Options, int _MaxRows, int _MaxCols>
inline void save(
	   Archive & ar,
	   const Eigen::Matrix<_Scalar, Eigen::Dynamic, _Cols, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	int rows = matrix.rows()  ;
	ar & rows ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Rows, int _Options, int _MaxRows, int _MaxCols>
inline void load(
	   Archive & ar,
	   Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	int cols ;
	ar & cols ;
	matrix.resize( _Rows, cols ) ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Rows, int _Options, int _MaxRows, int _MaxCols>
inline void save(
	   Archive & ar,
	   const Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	int cols = matrix.cols() ;
	ar & cols ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Options, int _MaxRows, int _MaxCols>
inline void load(
	   Archive & ar,
	   Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	int rows, cols ;
	ar & rows ;
	ar & cols ;
	matrix.resize( rows, cols ) ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Options, int _MaxRows, int _MaxCols>
inline void save(
	   Archive & ar,
	   const Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	int rows = matrix.rows(), cols = matrix.cols() ;
	ar & rows ;
	ar & cols ;
	ar & make_array( matrix.data(), matrix.size() ) ;
}

template<typename Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
inline void serialize(
	   Archive & ar,
	   Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> & matrix,
	   const unsigned int file_version
   )
{
	split_free( ar, matrix, file_version ) ;
}

#ifdef BOGUS_WITH_EIGEN_STABLE_SPARSE_API

template<typename Archive, typename _Scalar, int _Options, typename _Index >
inline void load(
	   Archive & ar,
	   Eigen::SparseMatrix<_Scalar, _Options, _Index> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	_Index rows, cols ;
	unsigned nnz ;
	ar & rows ;
	ar & cols ;
	ar & nnz ;
	matrix.resize( rows, cols ) ;
	matrix.resizeNonZeros( nnz ) ;
	ar & make_array( matrix.outerIndexPtr(), matrix.outerSize()+1 ) ;
	ar & make_array( matrix.innerIndexPtr(), nnz ) ;
	ar & make_array( matrix.valuePtr(), nnz ) ;
}

template<typename Archive, typename _Scalar, int _Options, typename _Index >
inline void save(
	   Archive & ar,
	   const Eigen::SparseMatrix<_Scalar, _Options, _Index> & matrix,
	   const unsigned int file_version
   )
{
	(void) file_version ;
	assert( matrix.isCompressed() ) ;

	_Index rows = matrix.rows(), cols = matrix.cols() ;
	unsigned nnz = matrix.data().size() ;
	ar & rows ;
	ar & cols ;
	ar & nnz ;
	ar & make_array( matrix.outerIndexPtr(), matrix.outerSize()+1 ) ;
	ar & make_array( matrix.innerIndexPtr(), nnz ) ;
	ar & make_array( matrix.valuePtr(), nnz ) ;
}

template<typename Archive, typename _Scalar, int _Options, typename _Index >
inline void serialize(
	   Archive & ar,
	   Eigen::SparseMatrix<_Scalar, _Options, _Index> & matrix,
	   const unsigned int file_version
   )
{
	split_free( ar, matrix, file_version ) ;
}

#endif

} // serialization
} // boost

#endif


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

#include "Math.hpp"

#include <boost/test/unit_test.hpp>

using namespace std;
using namespace toolbox;

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_SUITE(MathSuite)

BOOST_AUTO_TEST_CASE(NextPow2Case)
{
    BOOST_TEST(next_pow2(1U) == 1U);
    BOOST_TEST(next_pow2(2U) == 2U);
    BOOST_TEST(next_pow2(3U) == 4U);
    BOOST_TEST(next_pow2(4U) == 4U);
    BOOST_TEST(next_pow2(5U) == 8U);
    BOOST_TEST(next_pow2(6U) == 8U);
    BOOST_TEST(next_pow2(7U) == 8U);
    BOOST_TEST(next_pow2(8U) == 8U);

    BOOST_TEST(next_pow2(1UL) == 1UL);
    BOOST_TEST(next_pow2(2UL) == 2UL);
    BOOST_TEST(next_pow2(3UL) == 4UL);
    BOOST_TEST(next_pow2(4UL) == 4UL);
    BOOST_TEST(next_pow2(5UL) == 8UL);
    BOOST_TEST(next_pow2(6UL) == 8UL);
    BOOST_TEST(next_pow2(7UL) == 8UL);
    BOOST_TEST(next_pow2(8UL) == 8UL);
}

BOOST_AUTO_TEST_CASE(VarAccumCase, *utf::tolerance(0.0000001))
{
    VarAccum v;
    v.append(1345, 1301, 1368, 1322, 1310, 1370, 1318, 1350, 1303, 1299);
    BOOST_TEST(v.size() == 10U);
    BOOST_TEST(v.mean() == 1328.6);
    BOOST_TEST(var(v) == 754.2666667);
    BOOST_TEST(varp(v) == 678.84);
    BOOST_TEST(stdev(v) == 27.4639157);
    BOOST_TEST(stdevp(v) == 26.0545581);
    BOOST_TEST(v.min() == 1299);
    BOOST_TEST(v.max() == 1370);
}

BOOST_AUTO_TEST_CASE(CeilCase)
{
    BOOST_TEST(ceil(1U, 3U) == 1U);
    BOOST_TEST(ceil(2U, 3U) == 1U);
    BOOST_TEST(ceil(3U, 3U) == 1U);
    BOOST_TEST(ceil(4U, 3U) == 2U);
    BOOST_TEST(ceil(5U, 3U) == 2U);
    BOOST_TEST(ceil(6U, 3U) == 2U);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright 2020-2021 Telecom Paris

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include "transport_channel.h"

#include <cmath>

#include <algorithm>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/utility/setup/common_attributes.hpp>
#include <boost/log/utility/setup/file.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <limits>
#include <thread>
#include <vector>

#include "../../asn1c/nr_rrc/BCCH-DL-SCH-Message.h"
#include "../../utils/common_utils/common_utils.h"
#include "../../utils/sequence_generator/sequence_generator.h"
#include "../../variables/common_matrices/common_matrices.h"
#include "../../variables/common_variables/common_variables.h"
#include "../../variables/ldpc_matrices/ldpc_matrices.h"
#include "../libphy/libphy.h"

using namespace std;

auto free5GRAN::phy::transport_channel::compute_N_polar_code(int E,
                                                             int K,
                                                             int nmax) -> int {
  /**
   * \fn compute_N_polar_code
   * \brief Compute polar coding out sequence size
   * \standard TS 38.212 V15.2.0 Section 5.3.1
   *
   * \param[in] E: Rate matching output sequence length
   * \param[in] K: Polar coding input sequence length (including CRC)
   * \param[in] nmax: Maximum value of n
   *
   * \return Polar coding out sequence size
   */
  int n1, n2;
  float rmin = 1.0 / 8.0;
  if (E <= (9.0 / 8.0) * pow(2, ceil(log2(E)) - 1) &&
      (float)K / (float)E < 9.0 / 16.0) {
    n1 = ceil(log2(E)) - 1;
  } else {
    n1 = ceil(log2(E));
  }
  n2 = ceil(log2((float)K / rmin));
  if (nmax < 5 && n1 < 5 && n2 < 5) {
    return 5;
  } else {
    if (n1 < n2 && n1 < nmax) {
      return n1;
    } else if (n2 < n1 && n2 < nmax) {
      return n2;
    } else {
      return nmax;
    }
  }
}

void free5GRAN::phy::transport_channel::rate_recover(int* input_bits,
                                                     int* output_bits,
                                                     int i_bil,
                                                     int E,
                                                     int N,
                                                     int K) {
  /**
   * \fn rate_recover
   * \brief Rate recovering for polar coding.
   * \standard TS 38.212 V15.2.0 Section 5.4.1
   *
   * \partial_imp Code block de-interleaving not implemented
   * \param[in] input_bits: Input bits sequence
   * \param[out] output_bits: Output bits sequence
   * \param[in] i_bil: Coded blocks interleaving indicator (0: No interleaving,
   * 1: Interleaving) \param[in] E: Rate matching output sequence length
   * \param[in] N: Rate matching input sequence length
   */
  int e[E], y[N];
  /*
   * Coded bit-interleaving/de-interleaving TS38.212 5.4.1.3
   * No coded-bits interleaving
   */
  if (i_bil == 0) {
    for (int n = 0; n < E; n++) {
      e[n] = input_bits[n];
    }
  }
  /*
   * Bit de-selection TS38.212 5.4.1.2
   */
  if (E >= N) {
    for (int n = 0; n < N; n++) {
      y[n] = e[n];
    }
  } else {
    if ((float)K / (float)E <= 7.0 / 16.0) {
      for (int n = 0; n < N - E; n++) {
        y[n] = 0;
      }
      for (int n = 0; n < E; n++) {
        y[n + N - E] = e[n];
      }
    }
  }

  /*
   * Sub-block de-interleaving TS38.212 5.4.1.1
   */
  for (int n = 0; n < N; n++) {
    int i = floor(32 * (double)n / (double)N);
    int j_n =
        free5GRAN::SUB_BLOCK_INTERLEAVER_PATTERN[i] * N / 32 + n % (N / 32);
    output_bits[j_n] = y[n];
  }
}

void free5GRAN::phy::transport_channel::polar_decode(int* input_bits,
                                                     int* output_bits,
                                                     int N,
                                                     int K,
                                                     int nmax,
                                                     int i_il,
                                                     int n_pc,
                                                     int n_wm_pc,
                                                     int E) {
  /**
   * \fn polar_decode
   * \brief Polar decoding
   * \standard TS 38.212 V15.2.0 Section 5.3.1
   *
   * \partial_imp Only n_wm_pc=0 is implemented
   * \details
   * Details:
   * - Polar decoding using Gn inverse matrix
   * - Computing required sets
   * - Recover c'
   * - De-interleave c' to recover output
   *
   * \param[in] input_bits: Input bits sequence
   * \param[out] output_bits: Output bits sequence
   * \param[in] N: Rate matching input sequence length
   * \param[in] K: Polar coding input sequence length (including CRC)
   * \param[in] nmax: Maximum value of n
   * \param[in] i_il: Interleaving indicator (0: No interleaving, 1:
   * Interleaving) \param[in] n_pc: Number of parity check bits \param[in]
   * n_wm_pc: Number of other parity check bits
   */
  int q_0_n_1[N], count_seq, q_i_n[K + n_pc], c_p[K], pi_seq[K], j_n, u[N];
  int K_max = 164;
  bool found;

  /*
   * polar decoding using Gn inverse matrix (TS38.212 5.3.1.2)
   */
  switch (N) {
    case 32: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G5_INV[p][n]);
        }
      }
    };
    case 64: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G6_INV[p][n]);
        }
      }
    };
    case 128: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G7_INV[p][n]);
        }
      }
    };
    case 256: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G8_INV[p][n]);
        }
      }
    };
    case 512: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G9_INV[p][n]);
        }
      }
    };
    case 1024: {
      for (int n = 0; n < N; n++) {
        u[n] = 0;
        for (int p = 0; p < N; p++) {
          u[n] ^= (input_bits[p] * free5GRAN::G10_INV[p][n]);
        }
      }
    };
  }

  count_seq = 0;
  vector<int> q_ftmp_n, q_itmp_n;

  /*
   * Computing q_0_n_1 (TS38.212 5.3.1.2)
   */
  for (int n : free5GRAN::POLAR_SEQUENCE_QNMAX_AND_RELIABILITY) {
    if (n < N) {
      q_0_n_1[count_seq] = n;
      count_seq++;
    }
  }

  if (E < N) {
    if ((float)K / (float)E <= 7.0 / 16.0) {
      for (int n = 0; n < N - E; n++) {
        int i = floor(32 * (double)n / (double)N);
        j_n =
            free5GRAN::SUB_BLOCK_INTERLEAVER_PATTERN[i] * N / 32 + n % (N / 32);
        q_ftmp_n.push_back(j_n);
      }
      if (E >= 3.0 * (float)N / 4.0) {
        for (int n = 0; n < ceil(3.0 * (float)N / 4.0 - (float)E / 2.0); n++) {
          q_ftmp_n.push_back(n);
        }
      } else {
        for (int n = 0; n < ceil(9.0 * (float)N / 16.0 - (float)E / 4.0); n++) {
          q_ftmp_n.push_back(n);
        }
      }
    }
  }

  for (int n = 0; n < N; n++) {
    found = false;
    for (int x : q_ftmp_n) {
      if (q_0_n_1[n] == x) {
        found = true;
        break;
      }
    }
    if (!found) {
      q_itmp_n.push_back(q_0_n_1[n]);
    }
  }
  /*
   * Computing q_i_n (TS38.212 5.3.1.2)
   */
  for (int n = 0; n < K + n_pc; n++) {
    q_i_n[n] = q_itmp_n[q_itmp_n.size() - (K + n_pc) + n];
  }
  count_seq = 0;
  /*
   * Recovering c' from u_n (TS38.212 5.3.1.2)
   */
  for (int n = 0; n < N; n++) {
    found = false;
    for (int p = 0; p < K + n_pc; p++) {
      if (q_i_n[p] == n) {
        found = true;
        break;
      }
    }
    if (found) {
      c_p[count_seq] = u[n];
      count_seq++;
    }
  }
  count_seq = 0;
  /*
   * generating pi sequence (TS38.212 5.3.1.1)
   */
  for (int m = 0; m < K_max; m++) {
    if (free5GRAN::INTERLEAVING_PATTERN[m] >= K_max - K) {
      pi_seq[count_seq] = free5GRAN::INTERLEAVING_PATTERN[m] - (K_max - K);
      count_seq++;
    }
  }
  /*
   * de-interleaving c' to recover output sequence (TS38.212 5.3.1.1)
   */
  for (int k = 0; k < K; k++) {
    output_bits[pi_seq[k]] = c_p[k];
  }
}

void free5GRAN::phy::transport_channel::crc_validate(int* input_bits,
                                                     int* crc_polynom,
                                                     int* remainder,
                                                     int length_input,
                                                     int length_crc) {
  /**
   * \fn crc_validate
   * \brief CRC validation
   * \details
   * Computes the remainder of the division of the input sequence by the
   * polynom. If the remainder is equal to 0, CRC is validated
   *
   * \param[in] input_bits: Input bits sequence
   * \param[in] crc_polynom: Polynom used for CRC computation
   * \param[out] remainder: Output remainder of the division
   * \param[in] length_input: Input size
   * \param[in] length_crc: Polynom size
   */
  int num_steps, seq1[length_crc];

  /*
   * Getting index of first 1 in input_bits
   */
  int index_0 = -1;
  for (int i = 0; i < length_input; i++) {
    if (input_bits[i] == 1) {
      index_0 = i;
      break;
    }
  }
  /*
   * Creating a new input called temp_input by removing all 0 from the beginning
   * of input_bits
   */
  int temp_input[length_input - index_0];
  for (int i = 0; i < length_input - index_0; i++) {
    temp_input[i] = input_bits[index_0 + i];
  }
  length_input = length_input - index_0;
  num_steps = length_input - length_crc + 1;

  /*
   * seq1 is the variable that will be XORed with crc_polynom step after step
   * Initializing seq1 variable to the first bits of temp_input
   */
  for (int i = 0; i < length_crc; i++) {
    seq1[i] = temp_input[i];
  }

  int i = 0;

  /*
   * Iterate until the polynom reaches the end of temp_input
   */
  while (i < num_steps) {
    /*
     * XORing seq1 and crc_polynom
     */
    for (int j = 0; j < length_crc; j++) {
      remainder[j] = seq1[j] ^ crc_polynom[j];
    }
    /*
     * Checing if the remainder is equal to 0. If true, algorithm ends
     */
    bool validated = true;
    for (int j = 1; j < length_crc; j++) {
      if (remainder[j] == 1) {
        validated = false;
        break;
      }
    }
    if (validated) {
      break;
    }
    /*
     * Searching first 1 index in remainder
     */
    int index_1 = -1;
    for (int j = 1; j < length_crc; j++) {
      if (remainder[j] == 1) {
        index_1 = j;
        break;
      }
    }
    /*
     * seq1 is updated to be the remainder shifted until finding the first 1
     */
    for (int j = 0; j < length_crc - index_1; j++) {
      seq1[j] = remainder[j + index_1];
    }
    /*
     * Adding new entries from temp_input to fill seq1
     */
    for (int j = 0; j < index_1; j++) {
      seq1[length_crc - index_1 + j] = temp_input[length_crc + i + j];
    }
    i += index_1;
  }
}

void free5GRAN::phy::transport_channel::compute_crc(int* input_bits,
                                                    int* crc_polynom,
                                                    int* remainder,
                                                    int length_input,
                                                    int length_crc) {
  /**
   * \fn compute_crc
   * \brief CRC computation
   * \details
   * Compute the CRC for a given input and polynom
   *
   * \param[in] input_bits: Input bits sequence
   * \param[in] crc_polynom: Polynom used for CRC computation
   * \param[out] remainder: Output CRC
   * \param[in] length_input: Input size
   * \param[in] length_crc: Polynom size
   */
  int num_steps, *seq1;

  /*
   * Getting index of first 1 in input_bits
   */
  int index_0 = -1;
  for (int i = 0; i < length_input; i++) {
    if (input_bits[i] == 1) {
      index_0 = i;
      break;
    }
  }
  /*
   * Creating a new input called temp_input by removing all 0 from the beginning
   * of input_bits Adding (length_crc - 1) zeros to complete temp_input
   */
  int temp_input[length_input - index_0 + length_crc - 1];
  for (int i = 0; i < length_input - index_0; i++) {
    temp_input[i] = input_bits[index_0 + i];
  }
  for (int i = 0; i < length_crc - 1; i++) {
    temp_input[length_input - index_0 + i] = 0;
  }
  length_input = length_input - index_0;
  num_steps = length_input;

  /*
   * Iterate until the polynom reaches the end of temp_input
   */
  int i = 0;
  while (i < num_steps) {
    /*
     * XORing temp_input and crc_polynom
     */
    for (int j = 0; j < length_crc; j++) {
      temp_input[i + j] ^= crc_polynom[j];
    }
    /*
     * Checing if temp_input (for the first length_input bits) is equal to 0. If
     * true, algorithm ends
     */
    bool finished = true;
    for (int j = 0; j < length_input; j++) {
      if (temp_input[j] == 1) {
        finished = false;
        break;
      }
    }
    if (finished) {
      break;
    }
    /*
     * Shifting until finding a 1 (at least once)
     */
    i++;
    while (temp_input[i] != 1) {
      i++;
    }
  }
  /*
   * Remainder corresponds to the (length_crc - 1) last bits of temp_input
   */
  for (int j = 0; j < length_crc - 1; j++) {
    remainder[j] = temp_input[length_input + j];
  }
}

void free5GRAN::phy::transport_channel::compute_ldpc_base_graph(int A,
                                                                float R,
                                                                int& graph) {
  /**
   * \fn compute_ldpc_base_graph
   * \brief Choose LDPC base graph
   * \standard TS 38.212 V15.2.0 Section 7.2.2
   *
   * \param[in] A: Transport block size
   * \param[in] R: Code rate
   * \param[out] graph: Output graph
   */
  if (A <= 292 || (A <= 3824 && R <= 0.67) || R <= 0.25) {
    graph = 2;
  } else {
    graph = 1;
  }
}

void free5GRAN::phy::transport_channel::compute_transport_block_size(
    int n_re,
    float R,
    int mod_order,
    int num_layers,
    int nrb,
    int& tbs) {
  /**
   * \fn compute_transport_block_size
   * \brief Transport block size computation
   * \standard TS 38.214 V15.2.0 Section 5.1.3.2
   *
   * \partial_imp n_info > 3824 not implemented
   *
   * \param[in] n_re: Number of PDSCH RE per RB
   * \param[in] R: Code rate
   * \param[in] mod_order: Modulation order
   * \param[in] num_layers: Number of transport layers
   * \param[in] nrb: Number of RB in PDSCH allocation
   * \param[out] tbs: Transport block size
   */
  long nre = (long)min(156, n_re) * nrb;
  double n_info = (double)nre * R * mod_order * num_layers;
  if (n_info <= 3824) {
    long n = (int)max(3.0, floor(log2(n_info)) - 6);
    long n_p_info = max(24.0, pow(2, n) * floor((double)n_info / pow(2, n)));
    for (int i : free5GRAN::TS_38_214_TABLE_5_1_3_2_1) {
      if (i >= n_p_info) {
        tbs = i;
        break;
      }
    }
  } else {
    cout << "N INFO not supported !" << endl;
  }
}

/*
 * TS 38 212 5.2.2
 */
void free5GRAN::phy::transport_channel::compute_Zc_dl_sch(int kb,
                                                          float k_p,
                                                          int& Zc,
                                                          int& i_ls) {
  /**
   * \fn compute_Zc_dl_sch
   * \brief Compute LDPC lifting size for DL-SCH encoding/decoding
   * \standard TS 38.212 V15.2.0 Section 5.2.2
   *
   * \param[in] kb: Intermediate value for Zc computation
   * \param[in] k_p: K' value
   * \param[out] Zc: Returned value
   * \param[out] i_ls: Zc set index
   */
  Zc = 512;
  for (int i = 0; i < 8; i++) {
    for (int j = 0; j < 8; j++) {
      if (TS_38_212_TABLE_5_3_2_1[i][j] < Zc &&
          TS_38_212_TABLE_5_3_2_1[i][j] >= (k_p / (float)kb)) {
        Zc = TS_38_212_TABLE_5_3_2_1[i][j];
        i_ls = i;
      }
    }
  }
}

void free5GRAN::phy::transport_channel::
    compute_code_block_segmentation_info_ldpc(int graph,
                                              int B,
                                              int& Zc,
                                              int& K,
                                              int& i_ls,
                                              int& L,
                                              int& C,
                                              int& N,
                                              int& K_p) {
  /**
   * \fn compute_code_block_segmentation_info_ldpc
   * \brief Compute code block segmentation informations for LDPC
   * \standard TS 38.212 V15.2.0 Section 5.2.2
   *
   * \param[in] graph: LDPC base graph
   * \param[in] B: Code block segmentation input size (inlcuding CRC)
   * \param[out] Zc: Returned LDPC lifting size
   * \param[out] K: Code block segmentation output size
   * \param[out] i_ls: Zc set index
   * \param[out] L: Code block CRC length
   * \param[out] C: Number of code blocks
   * \param[out] N: LDPC output sequence
   * \param[out] K_p: K' intermediate K value
   */
  int k_b, B_p;
  int k_cb = (graph == 1) ? 8448 : 3840;
  if (B <= k_cb) {
    L = 0;
    C = 1;
    B_p = B;
  } else {
    L = 24;
    C = ceil(B / (k_cb - L));
    B_p = B + C * L;
  }
  float k_p = (float)B_p / (float)C;
  if (graph == 1) {
    k_b = 22;
  } else {
    if (B > 640) {
      k_b = 10;
    } else if (B > 560) {
      k_b = 9;
    } else if (B > 192) {
      k_b = 8;
    } else {
      k_b = 6;
    }
  }
  compute_Zc_dl_sch(k_b, k_p, Zc, i_ls);
  K = (graph == 1) ? 22 * Zc : 10 * Zc;
  N = (graph == 1) ? 66 * Zc : 50 * Zc;
  K_p = (int)k_p;
}

void free5GRAN::phy::transport_channel::rate_recover_ldpc(
    int* input_bits,
    int N,
    int i_lbrm,
    int E,
    int id_rv,
    int mod_order,
    int C,
    int Zc,
    int graph,
    int K,
    int K_p,
    int* output_sequence) {
  /**
   * \fn rate_recover_ldpc
   * \brief Rate recovering for LDPC (hard bits)
   * \standard TS 38.212 V15.2.0 Section 5.4.2
   *
   * \param[in] input_bits: DL-SCH input bits
   * \param[in] N: Output sequence length
   * \param[in] i_lbrm: Indicator for N_cb computation
   * \param[in] E: DL-SCH input sequence length
   * \param[in] id_rv: Redundancy version
   * \param[in] mod_order: Modulation order
   * \param[in] C: Number of code blocks
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] K: Code block length
   * \param[in] K_p: K' code block intermediate length
   * \param[out] output_sequence: Output bits
   */
  int N_cb = (i_lbrm == 0) ? N : min(N, 25344);
  int e[E];
  int E_Q = (int)((float)E / (float)mod_order);
  /*
   * Input de-interleaving
   */
  for (int j = 0; j < E_Q; j++) {
    for (int i = 0; i < mod_order; i++) {
      e[i * (E_Q) + j] = input_bits[i + j * mod_order];
    }
  }
  /*
   * Compute k0
   */
  int k0, k, j;
  if (id_rv == 0) {
    k0 = 0;
  } else if (id_rv == 1) {
    k0 = (graph == 1) ? floor((17.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((13.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 2) {
    k0 = (graph == 1) ? floor((33.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((25.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 3) {
    k0 = (graph == 1) ? floor((56.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((43.0 * N_cb) / (50.0 * Zc)) * Zc;
  }

  /*
   * rate recovering
   */
  j = 0;
  k = 0;
  while (k < E) {
    int index = (k0 + j) % N_cb;
    if (index >= K_p - 2 * Zc && index < K - 2 * Zc) {
      output_sequence[index] = -1;
    } else {
      output_sequence[index] = e[k];
      k++;
    }
    j++;
  }
}

void free5GRAN::phy::transport_channel::rate_recover_ldpc(
    double* input_bits,
    int N,
    int i_lbrm,
    int E,
    int id_rv,
    int mod_order,
    int C,
    int Zc,
    int graph,
    int K,
    int K_p,
    double* output_sequence) {
  /**
   * \fn rate_recover_ldpc
   * \brief Rate recovering for LDPC (soft bits)
   * \standard TS 38.212 V15.2.0 Section 5.4.2
   *
   * \param[in] input_bits: DL-SCH input soft bits
   * \param[in] N: Output sequence length
   * \param[in] i_lbrm: Indicator for N_cb computation
   * \param[in] E: DL-SCH input sequence length
   * \param[in] id_rv: Redundancy version
   * \param[in] mod_order: Modulation order
   * \param[in] C: Number of code blocks
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] K: Code block length
   * \param[in] K_p: K' code block intermediate length
   * \param[out] output_sequence: Output soft bits
   */
  int N_cb = (i_lbrm == 0) ? N : min(N, 25344);
  double e[E];
  int E_Q = (int)((float)E / (float)mod_order);
  /*
   * Input de-interleaving
   */
  for (int j = 0; j < E_Q; j++) {
    for (int i = 0; i < mod_order; i++) {
      e[i * (E_Q) + j] = input_bits[i + j * mod_order];
    }
  }

  int k0, k, index, j;
  if (id_rv == 0) {
    k0 = 0;
  } else if (id_rv == 1) {
    k0 = (graph == 1) ? floor((17.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((13.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 2) {
    k0 = (graph == 1) ? floor((33.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((25.0 * N_cb) / (50.0 * Zc)) * Zc;
  } else if (id_rv == 3) {
    k0 = (graph == 1) ? floor((56.0 * N_cb) / (66.0 * Zc)) * Zc
                      : floor((43.0 * N_cb) / (50.0 * Zc)) * Zc;
  }

  vector<int> seen_indexes;
  /*
   * rate recovering
   */
  j = 0;
  k = 0;
  while (k < E) {
    index = (k0 + j) % N_cb;
    if (find(seen_indexes.begin(), seen_indexes.end(), index) ==
        seen_indexes.end()) {
      seen_indexes.push_back(index);
      if (index >= K_p - 2 * Zc && index < K - 2 * Zc) {
        output_sequence[index] = numeric_limits<double>::infinity();
        ;
      } else {
        output_sequence[index] = e[k];
        k++;
      }
    }
    j++;
  }
}

void free5GRAN::phy::transport_channel::compute_circular_permutation_matrix(
    int size,
    int offset,
    int** matrix) {
  /**
   * \fn compute_circular_permutation_matrix
   * \brief Compute offset times right shifted identity matrix of size n
   * \param[in] size: Identity matrix size
   * \param[in] offset: Number of times to shift matrix
   * \param[out] matrix: Output matrix
   */
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < size; j++) {
      matrix[i][j] = 0;
    }
    matrix[i][(i + offset) % size] = 1;
  }
}

void free5GRAN::phy::transport_channel::ldpc_decode_one_bit(
    vector<vector<int>> R,
    double* soft_bits,
    int i,
    double& new_bit) {
  /**
   * \fn ldpc_decode_one_bit
   * \brief LDPC correct one bit value (using belief propagation algorithm)
   * \param[in] R: Input set of rows connected to bit i
   * \param[in] soft_bits: Input soft bits
   * \param[in] i: Bit index
   * \param[out] new_bit: New bit value
   */
  double r_p1[R.size()], r_prop, q_p1, q_m1, n_q_p1, n_q_m1;
  /*
   * Compute rj for each element in R
   */
  for (int j = 0; j < R.size(); j++) {
    r_p1[j] = 1;
    for (int i_p = 0; i_p < R[j].size(); i_p++) {
      r_prop = 1 / (1 + exp(2 * soft_bits[R[j][i_p]]));
      r_p1[j] *= 1 - 2 * r_prop;
    }
    r_p1[j] = 0.5 + 0.5 * r_p1[j];
  }
  /*
   * Compute updated bit probabilities
   */
  q_m1 = 1;
  q_p1 = 1;
  for (int j = 0; j < R.size(); j++) {
    q_p1 *= r_p1[j];
    q_m1 *= (1 - r_p1[j]);
  }
  r_prop = 1 / (1 + exp(2 * soft_bits[i]));
  q_m1 = r_prop * q_m1;
  q_p1 = (1 - r_prop) * q_p1;
  /*
   * Normalization
   */
  n_q_p1 = q_p1 / (q_p1 + q_m1);
  n_q_m1 = q_m1 / (q_p1 + q_m1);
  /*
   * Update bit value depending on max probability
   */
  if (n_q_p1 > n_q_m1) {
    new_bit = -0.5 * log((1 / n_q_p1) - 1);
  } else {
    new_bit = 0.5 * log((1 / n_q_m1) - 1);
  }
}

void free5GRAN::phy::transport_channel::compute_H_matrix_ldpc(
    int Zc,
    int graph,
    int i_ls,
    vector<vector<int>>& matrix,
    int& size_i,
    int& size_j) {
  /**
   * \fn compute_H_matrix_ldpc
   * \brief Compute Hbg matrix for LDPC
   * \standard TS 38.212 V15.2.0 Section 5.3.2
   *
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] i_ls: Zc set index
   * \param[out] matrix: H matrix
   * \param[out] size_i: H matrix number of rows
   * \param[out] size_i: H matrix number of columns
   */
  if (graph == 1) {
    size_i = 46;
    size_j = 68;
  } else {
    size_i = 42;
    size_j = 52;
  }

  vector<int*> ldpc_table;
  if (graph == 1) {
    ldpc_table = free5GRAN::TS_38_212_TABLE_5_3_2_2;
  } else {
    ldpc_table = free5GRAN::TS_38_212_TABLE_5_3_2_3;
  }
  for (auto& p : ldpc_table) {
    int index_i = p[0];
    int index_j = p[1];
    int v_ij = p[i_ls + 2];
    matrix[index_i][index_j] = v_ij;
  }
}

void free5GRAN::phy::transport_channel::ldpc_decode(double* input_bits,
                                                    int N,
                                                    int Zc,
                                                    int graph,
                                                    int K,
                                                    int i_ls,
                                                    int* output_sequence) {
  /**
   * \fn compute_H_matrix_ldpc
   * \brief Compute H matrix for LDPC using Belief propagation algorithm
   * \standard TS 38.212 V15.2.0 Section 5.3.2
   *
   * \details
   * Details:
   * - Compute H matrix
   * - Recover LDPC input bits from input sequence
   * - Compute columns and row bit relations
   * - Iterate 10 time:
   * -# Try to correct each input bit value
   * -# If H times corrected bits value equals 0, algorithm ends, otherwise it
   * continues
   *
   * \param[in] input_bits: Input soft bits sequence
   * \param[in] N: LDPC input bit sequence length
   * \param[in] Zc: LDPC lifting size
   * \param[in] graph: LDPC base graph
   * \param[in] K: Code block length
   * \param[in] i_ls: Zc set index
   * \param[out] output_sequence: Corrected output bits sequence
   */
  /*
   * H matrix generation
   */
  int size_i, size_j;
  size_i = 52;
  size_j = 42;
  vector<vector<int>> H(size_j, vector<int>(size_i, -1));
  compute_H_matrix_ldpc(Zc, graph, i_ls, H, size_j, size_i);

  double ldpc_input_bits[N + 2 * Zc];
  for (int i = 0; i < 2 * Zc; i++) {
    ldpc_input_bits[i] = 0;
  }
  for (int k = 2 * Zc; k < N + 2 * Zc; k++) {
    ldpc_input_bits[k] = input_bits[k - 2 * Zc];
  }
  double new_bits[N + 2 * Zc];
  vector<vector<int>> R[N + 2 * Zc], R_tot;
  vector<int> new_vec, new_vec_tot;

  /*
   * Generate rows and columns matrices
   */
  for (int j = 0; j < size_j * Zc; j++) {
    new_vec_tot.clear();
    for (int i = 0; i < size_i; i++) {
      if (H[j / Zc][i] != -1) {
        new_vec_tot.push_back(((j % Zc + H[j / Zc][i]) % Zc) + i * Zc);
      }
    }
    R_tot.push_back(new_vec_tot);
  }
  int index_l, inter_ind;
  for (int i = 0; i < N + 2 * Zc; i++) {
    for (int j = 0; j < size_j; j++) {
      if (H[j][i / Zc] != -1) {
        new_vec.clear();
        inter_ind = ((i % Zc - H[j][i / Zc]) % Zc);
        if (inter_ind < 0) {
          inter_ind = Zc + inter_ind;
        }
        index_l = inter_ind + j * Zc;

        for (int& p : R_tot[index_l]) {
          if (p != i) {
            new_vec.push_back(p);
          }
        }
        R[i].push_back(new_vec);
      }
    }
  }
  int rest, final_bit[N + 2 * Zc];
  /*
   * Looping over iteration
   */
  for (int iter = 0; iter < 10; iter++) {
    /*
     * Decode bits
     */
    for (int i = 0; i < N + 2 * Zc; i++) {
      ldpc_decode_one_bit(R[i], &ldpc_input_bits[0], i, ref(new_bits[i]));
    }
    for (int i = 0; i < N + 2 * Zc; i++) {
      ldpc_input_bits[i] = new_bits[i];
      final_bit[i] = (ldpc_input_bits[i] < 0) ? 1 : 0;
      if (i < K) {
        output_sequence[i] = final_bit[i];
      }
    }

    bool validated = true;
    for (int j = 0; j < size_j * Zc; j++) {
      rest = 0;
      for (int i : R_tot[j]) {
        rest ^= final_bit[i];
      }
      if (rest == 1) {
        validated = false;
        break;
      }
    }
    if (validated) {
      BOOST_LOG_TRIVIAL(trace) << "LDPC VALIDATED";
      break;
    }
  }
}

void free5GRAN::phy::transport_channel::decode_bch(int* bch_bits,
                                                   bool& crc_validated,
                                                   int* mib_bits,
                                                   int pci) {
  /**
   * \fn decode_bch
   * \brief Decode broadcast channel
   * \standard TS 38.212 V15.2.0 Section 7.1
   *
   * \details
   * Details:
   * - Rate recovering
   * - Polar decoding
   * - CRC validation
   * - Scrambling
   * - Payload de-interleaving
   *
   * \param[in] bch_bits: Input bits sequence
   * \param[out] crc_validated: Indicator for CRC validation
   * \param[out] mib_bits: Output MIB bits
   * \param[in] pci: Cell PCI
   */
  int n = free5GRAN::phy::transport_channel::compute_N_polar_code(
      free5GRAN::SIZE_SSB_PBCH_SAMPLES * 2, free5GRAN::SIZE_PBCH_POLAR_DECODED,
      9);
  int N = pow(2, n);
  int rate_recovered_bits[N],
      polar_decoded_bits[free5GRAN::SIZE_PBCH_POLAR_DECODED],
      remainder[free5GRAN::BCH_CRC_LENGTH + 1],
      bch_payload[free5GRAN::BCH_PAYLOAD_SIZE],
      bch_crc_recomputed[free5GRAN::BCH_CRC_LENGTH],
      bch_crc[free5GRAN::BCH_CRC_LENGTH], crc_masq[free5GRAN::BCH_CRC_LENGTH];
  // Rate recover bch_bits to rate_recovered_bits
  free5GRAN::phy::transport_channel::rate_recover(
      bch_bits, rate_recovered_bits, 0, free5GRAN::SIZE_SSB_PBCH_SAMPLES * 2, N,
      free5GRAN::SIZE_PBCH_POLAR_DECODED);
  // Polar decode rate_recovered_bits to polar_decoded_bits
  free5GRAN::phy::transport_channel::polar_decode(
      rate_recovered_bits, polar_decoded_bits, N,
      free5GRAN::SIZE_PBCH_POLAR_DECODED, 9, 1, 0, 0,
      free5GRAN::SIZE_SSB_PBCH_SAMPLES * 2);
  // Validate polar_decoded_bits CRC (compute the remainder and check that t is
  // equal to 0)
  free5GRAN::phy::transport_channel::crc_validate(
      polar_decoded_bits, free5GRAN::G_CRC_24_C, remainder,
      free5GRAN::SIZE_PBCH_POLAR_DECODED, free5GRAN::BCH_CRC_LENGTH + 1);
  crc_validated = true;
  for (int i = 1; i < free5GRAN::BCH_CRC_LENGTH + 1; i++) {
    if (remainder[i] == 1) {
      crc_validated = false;
      break;
    }
  }
  // Split polar_decoded_bits into bch_payload and bch_crc
  for (int i = 0; i < free5GRAN::BCH_PAYLOAD_SIZE; i++) {
    bch_payload[i] = polar_decoded_bits[i];
  }
  for (int i = 0; i < free5GRAN::BCH_CRC_LENGTH; i++) {
    bch_crc[i] = polar_decoded_bits[free5GRAN::BCH_PAYLOAD_SIZE + i];
  }
  // Re-compute the bch_payload CRC
  free5GRAN::phy::transport_channel::compute_crc(
      bch_payload, free5GRAN::G_CRC_24_C, bch_crc_recomputed, 32, 25);

  // XOR recomputed CRC with received CRC to determine CRC masq
  for (int i = 0; i < free5GRAN::BCH_CRC_LENGTH; i++) {
    crc_masq[i] = bch_crc[i] ^ bch_crc_recomputed[i];
  }

  /*
   * PBCH payload recovering (TS38.212 7.1.1)
   */
  int A = free5GRAN::BCH_PAYLOAD_SIZE;
  int A_bar = free5GRAN::BCH_PAYLOAD_SIZE - 8;
  int M = A - 3;
  int s_sequence[A], bch_descrambled[free5GRAN::BCH_PAYLOAD_SIZE];
  int sfn_bits[4][2] = {{0, 0}, {0, 1}, {1, 0}, {1, 1}};
  // Find the correct value of v
  for (int v = 0; v < 4; v++) {
    // Generate de-scrambling sequence
    int c_seq[free5GRAN::BCH_PAYLOAD_SIZE + v * M];
    free5GRAN::utils::sequence_generator::generate_c_sequence(
        pci, free5GRAN::BCH_PAYLOAD_SIZE + v * M, c_seq, 0);
    int j = 0;
    // Generate s sequence
    for (int i = 0; i < A; i++) {
      if (i == 0 || i == 6 || i == 24) {
        s_sequence[i] = 0;
      } else {
        s_sequence[i] = c_seq[j + v * M];
        j++;
      }
    }
    // De-scramble bch_payload to bch_descrambled
    free5GRAN::utils::common_utils::scramble(bch_payload, s_sequence,
                                             bch_descrambled,
                                             free5GRAN::BCH_PAYLOAD_SIZE, 0);

    // BCH payload de-interleaving
    int j_sfn = 0;
    int j_hrf = 10;
    int j_ssb = 11;
    int j_other = 14;
    for (int i = 0; i < 32; i++) {
      if (i == 24 || i == 25 || i == 26 || i == 27 || i == 1 || i == 2 ||
          i == 3 || i == 4 || i == 5 || i == 6) {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_sfn]];
        j_sfn++;
      } else if (i == 28) {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_hrf]];
      } else if (i >= A_bar + 5 && i <= A_bar + 7) {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_ssb]];
        j_ssb++;
      } else {
        mib_bits[i] =
            bch_descrambled[free5GRAN::PBCH_PAYLOAD_INTERLEAVER[j_other]];
        j_other++;
      }
    }
    // If final bits correspond to 3rd and 2nd LSB of SFN, correct v was found
    if (sfn_bits[v][0] == mib_bits[25] && sfn_bits[v][1] == mib_bits[26]) {
      break;
    }
  }
}

void free5GRAN::phy::transport_channel::decode_dci(
    int* dci_bits,
    int E,
    int K,
    int* rnti,
    bool& validated,
    vector<int>& decoded_dci_bits) {
  /**
   * \fn decode_dci
   * \brief DCI decoding
   * \standard TS 38.212 V15.2.0 Section 7.3
   *
   * \partial_imp Only DCI Format 1_0 has been tested
   * \details
   * Details:
   * - Rate recovering
   * - Polar decoding
   * - RNTI scrambling
   * - CRC validation
   *
   * \param[in] dci_bits: Input bits sequence
   * \param[in] E: PDCCH payload size
   * \param[in] K: DCI payload size (including CRC)
   * \param[in] rnti: RNTI for identifying DCI Format
   * \param[out] validated: Indicator for CRC validation
   * \param[out] decoded_dci_bits: Output bits sequence
   */
  int n = compute_N_polar_code(E, K, 9);
  int N = pow(2, n);

  BOOST_LOG_TRIVIAL(trace) << "(n, N) = (" + to_string(n) + ", " +
                                  to_string(N) + ")";
  BOOST_LOG_TRIVIAL(trace) << "E = " + to_string(E);
  BOOST_LOG_TRIVIAL(trace) << "K = " + to_string(K);

  int rate_recovered[N], polar_decoded[K], remainder[25], descrambled[K + 24];

  int A = K - 24;
  /*
   * rate recovering
   */
  rate_recover(dci_bits, rate_recovered, 0, E, N, K);
  /*
   * Polar decoding
   */
  polar_decode(rate_recovered, polar_decoded, N, K, 9, 1, 0, 0, E);
  /*
   * RNTI de-masking and CRC validation
   */
  for (int i = 0; i < 24; i++) {
    descrambled[i] = 1;
  }
  for (int i = 0; i < K; i++) {
    if (i < A + 8) {
      descrambled[i + 24] = polar_decoded[i];
    } else {
      descrambled[i + 24] = (polar_decoded[i] + rnti[i - A - 8]) % 2;
    }
  }
  crc_validate(descrambled, free5GRAN::G_CRC_24_C, remainder, K + 24, 25);
  validated = true;
  for (int i : remainder) {
    if (i == 1) {
      validated = false;
      break;
    }
  }
  BOOST_LOG_TRIVIAL(trace) << "## CRC "
                           << ((validated) ? "validated" : "not validated");

  for (int i = 0; i < A; i++) {
    decoded_dci_bits[i] = polar_decoded[i];
  }
}

auto free5GRAN::phy::transport_channel::decode_dl_sch(
    double* dl_sch_bits,
    int n_re,
    float R,
    int nrb,
    int E,
    bool& validated,
    free5GRAN::dci_1_0_si_rnti dci_1_0_si_rnti) -> vector<int> {
  /**
   * \fn decode_dl_sch
   * \brief DL-SCH decoding
   * \standard TS 38.212 V15.2.0 Section 7.2
   *
   * \partial_imp Implemented for one code block (C=1)
   * \details
   * Details:
   * - Rate recovering
   * - LDPC decoding
   * - CRC validation
   *
   * \param[in] dl_sch_bits: Input soft bits sequence
   * \param[in] n_re: Number of RE per PDSCH RB
   * \param[in] R: Code Rate
   * \param[in] nrb: Number of PDSCH allocated RB
   * \param[in] E: PDSCH output sequence length
   * \param[out] validated: CRC validation indicator
   * \param[in] dci_1_0_si_rnti: Input DCI Format 1_0 object
   */

  /*
   * Compute code block segmentation information
   */
  int graph, A, N, K, Zc, i_ls, L_cb, C, B, L, K_p;
  compute_transport_block_size(n_re, R, 2, 1, nrb, A);
  compute_ldpc_base_graph(A, R, graph);
  L = (A > 3824) ? 24 : 16;
  B = A + L;
  compute_code_block_segmentation_info_ldpc(graph, B, Zc, K, i_ls, L_cb, C, N,
                                            K_p);

  BOOST_LOG_TRIVIAL(trace) << "(TBS, graph) = " << A << ", " << graph;
  BOOST_LOG_TRIVIAL(trace) << "B = " << B;
  BOOST_LOG_TRIVIAL(trace) << "Zc = " << Zc;
  BOOST_LOG_TRIVIAL(trace) << "K = " << K;
  BOOST_LOG_TRIVIAL(trace) << "i_ls = " << i_ls;
  BOOST_LOG_TRIVIAL(trace) << "L = " << L;
  BOOST_LOG_TRIVIAL(trace) << "C = " << C;
  BOOST_LOG_TRIVIAL(trace) << "N = " << N;
  BOOST_LOG_TRIVIAL(trace) << "L_cb = " << L_cb;
  BOOST_LOG_TRIVIAL(trace) << "K_p = " << K_p;
  BOOST_LOG_TRIVIAL(trace) << "E = " << E;

  /*
   * Rate recovering
   */
  auto* rate_recovered = new double[N];
  rate_recover_ldpc(dl_sch_bits, N, 1, E, dci_1_0_si_rnti.rv, 2, C, Zc, graph,
                    K, K_p, rate_recovered);

  /*
   * LDPC decoding
   */
  int ldpc_decoded[K];
  auto start = chrono::steady_clock::now();
  ldpc_decode(rate_recovered, N, Zc, graph, K, i_ls, ldpc_decoded);
  auto end = chrono::steady_clock::now();
  auto diff = end - start;
  BOOST_LOG_TRIVIAL(trace) << "## LDPC execution time "
                           << chrono::duration<double, milli>(diff).count()
                           << "ms";

  int desegmented[B];
  for (int i = 0; i < B; i++) {
    desegmented[i] = ldpc_decoded[i];
  }

  /*
   * CRC validation
   */
  int remainder[L];
  if (L == 24) {
    crc_validate(desegmented, free5GRAN::G_CRC_24_C, remainder, B, L + 1);
  } else {
    crc_validate(desegmented, free5GRAN::G_CRC_16, remainder, B, L + 1);
  }

  validated = true;
  for (int i = 0; i < L + 1; i++) {
    if (remainder[i] == 1) {
      validated = false;
      break;
    }
  }
  vector<int> output_bits(A);

  for (int i = 0; i < A; i++) {
    output_bits[i] = desegmented[i];
  }

  BOOST_LOG_TRIVIAL(trace) << "## CRC "
                           << ((validated) ? "validated" : "not validated");

  return output_bits;
}


#include <iostream>
#include <string>
#include <Eigen/Dense>
#include "surface_recon.hpp"
#include <ros/ros.h>

bool check_pca_threshold(std::vector<Point_3> pts, float threshold,
                         int max); //compares ratio between largest and smallest eigenvalue
void rm_artifacts_mesh(std::string path, int k);

CPoint3 laplacian_smooth(std::vector<Point_3> pts);


/*
 * @Description: ceres residual block for sliding window marginalization
 * @Author: Ge Yao
 * @Date: 2021-01-05 21:57:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MARGINALIZATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MARGINALIZATION_HPP_

#include <ceres/ceres.h>

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

#include <sophus/so3.hpp>

#include "glog/logging.h"

namespace sliding_window {

class FactorPRVAGMarginalization : public ceres::SizedCostFunction<15, 15> {
public:
	static const int INDEX_M =  0;
  static const int INDEX_R = 15;

  FactorPRVAGMarginalization(void) {
    H_ = Eigen::MatrixXd::Zero(30, 30);
    b_ = Eigen::VectorXd::Zero(30);

    J_ = Eigen::MatrixXd::Zero(15, 15);
    e_ = Eigen::VectorXd::Zero(15);
  }

  void SetResMapMatchingPose(
    const ceres::CostFunction *residual,
    const std::vector<double *> &parameter_blocks
  ) {
    // init:
    ResidualBlockInfo res_map_matching_pose(residual, parameter_blocks);
    Eigen::VectorXd residuals;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;

    // compute:
    Evaluate(res_map_matching_pose, residuals, jacobians);
    const Eigen::MatrixXd &J_m = jacobians.at(0);

    //
    // TODO: Update H:
    //
    // a. H_mm:

    //
    // TODO: Update b:
    //
    // a. b_m:
  }

  void SetResRelativePose(
    const ceres::CostFunction *residual,
    const std::vector<double *> &parameter_blocks
  ) {
    // init:
    ResidualBlockInfo res_relative_pose(residual, parameter_blocks);
    Eigen::VectorXd residuals;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;

    // compute:
    Evaluate(res_relative_pose, residuals, jacobians);
    const Eigen::MatrixXd &J_m = jacobians.at(0);
    const Eigen::MatrixXd &J_r = jacobians.at(1);

    //
    // TODO: Update H:
    //
    // a. H_mm:
    // b. H_mr:
    // c. H_rm:
    // d. H_rr:

    //
    // TODO: Update b:
    //
    // a. b_m:
    // a. b_r:
  }

  void SetResIMUPreIntegration(
    const ceres::CostFunction *residual,
    const std::vector<double *> &parameter_blocks
  ) {
    // init:
    ResidualBlockInfo res_imu_pre_integration(residual, parameter_blocks);
    Eigen::VectorXd residuals;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;

    // compute:
    Evaluate(res_imu_pre_integration, residuals, jacobians);
    const Eigen::MatrixXd &J_m = jacobians.at(0);
    const Eigen::MatrixXd &J_r = jacobians.at(1);

    //
    // TODO: Update H:
    //
    // a. H_mm:
    // b. H_mr:
    // c. H_rm:
    // d. H_rr:

    //
    // Update b:
    //
    // a. b_m:
    // a. b_r:
  }

  void Marginalize(
    const double *raw_param_r_0
  ) {
    // TODO: implement marginalization logic
  }

  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    //
    // parse parameters:
    //
    Eigen::Map<const Eigen::Matrix<double, 15, 1>> x(parameters[0]);
    Eigen::VectorXd dx = x - x_0_;

    //
    // TODO: compute residual:
    //

    //
    // TODO: compute jacobian:
    //
    if ( jacobians ) {
      if ( jacobians[0] ) {
        // implement computing:
      }
    }

    return true;
  }

private:
  Eigen::MatrixXd H_;
  Eigen::VectorXd b_;

  Eigen::MatrixXd J_;
  Eigen::VectorXd e_;

  Eigen::VectorXd x_0_;

  struct ResidualBlockInfo {
    const ceres::CostFunction *residual = nullptr;
    std::vector<double *> parameter_blocks;

    ResidualBlockInfo(void) {}

    ResidualBlockInfo(
      const ceres::CostFunction *_residual,
      const std::vector<double *> &_parameter_blocks
    ) : residual(_residual), parameter_blocks(_parameter_blocks) {}
  };

  static void Evaluate(
    ResidualBlockInfo &residual_info,
    Eigen::VectorXd &residuals,
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> &jacobians
  ) {
    // init residual output:
    const int D = static_cast<int>(residual_info.residual->num_residuals());
    residuals.resize(D);

    // init jacobians output:
    std::vector<int> block_sizes = residual_info.residual->parameter_block_sizes();
    const int N = static_cast<int>(block_sizes.size());

    double **raw_jacobians = new double *[N];
    jacobians.resize(N);

    // create raw pointer adaptor:
    for (int i = 0; i < N; i++) {
      jacobians[i].resize(D, block_sizes[i]);
      raw_jacobians[i] = jacobians[i].data();
    }

    residual_info.residual->Evaluate(
      residual_info.parameter_blocks.data(),
      residuals.data(), raw_jacobians
    );
  }
};

} // namespace sliding_window

#endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MARGINALIZATION_HPP_


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_LOG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_LOG_HPP_INCLUDED

#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/fma.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/simd/if_nan_else.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/if_else_zero.hpp>
#include <boost/simd/function/simd/is_ltz.hpp>
#include <boost/simd/arch/common/detail/generic/f_log_kernel.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/log2_em1.hpp>
#include <boost/simd/constant/log10_ehi.hpp>
#include <boost/simd/constant/log10_elo.hpp>
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/constant/log10_2lo.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/simd/is_nan.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#endif
#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/is_less.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/constant/mlogtwo2nmb.hpp>
#include <boost/simd/constant/mlog2two2nmb.hpp>
#include <boost/simd/constant/mlog10two2nmb.hpp>
#endif

  //////////////////////////////////////////////////////////////////////////////
  // how to compute the various logarithms
  //////////////////////////////////////////////////////////////////////////////
  // The method is mainly taken from the cephes library:
  // first reduce the the data
  // a0 is supposed > 0
  // the input a0 is split into a mantissa and an exponent
  // the mantissa m is between sqrt(0.5) and sqrt(2) and the corresponding exponent is e
  // a0 = m*2^e
  // then the log? calculus is split in two parts (? being nothing: natural logarithm,  2: base 2 logarithm,
  //10 base ten logarithm)
  // as log?(a) = log?(2^e)+log?(m)
  // 1) computing log?(m)
  //   first put x = m-1 (so -0.29 <  x < 0.414)
  //   write log(m)   = log(1+x)   = x + x*x/2 + x*x*x*g(x)
  //   write log2(m)  = log2(1+x)  = C2*log(x),
  //     C2 = log(2)  the multiplication have to be taken seriously as C2 is not exact
  //   write log10(m) = log10(1+x) = C10*log(x),
  //     C10= log(10) the multiplication have to be taken seriously as C10 is not exact
  // then g(x) has to be approximated
  // g is ((log(1+x)/x-1)/x-1/2)/x
  // It is not a good idea to approximate directly log(1+x) instead of g,  because this will lead to bad precision around 1.
  //
  // in this approximation one can choose a best approximation rational function given by remez algorithm.
  // there exist a classical solution which is a polynomial p8 one of degree 8 that gives 0.5ulps everywhere
  // this is what is done in the kernel_t::log impl;
  // Now,  it is possible to choose a rational fraction or a polynomial of lesser degree to approximate g
  // providing faster but less accurate logs.
  // 2) computing log?(2^e)
  // see the explanations relative to each case
  // 3) finalize
  // This is simply treating invalid entries
  // 4) For denormal we use the fact that log(x) =  log?(x*y)-log?(y) and that if y is
  // the constant two2nmb if x is denormal x*y and y are not.
  //////////////////////////////////////////////////////////////////////////////

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd = boost::dispatch;
    template < class A0 >
    struct logarithm< A0, tag::simd_type, float>
    {
      using int_type = bd::as_integer_t<A0, signed>;
      using sA0 = bd::scalar_of_t<A0>;
      using  kernel_t = kernel<A0, tag::simd_type, float>;

      static inline A0 log(const A0& a0) BOOST_NOEXCEPT
      {
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        auto denormal = is_less(bs::abs(z), Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        t = if_else_zero(denormal, Mlogtwo2nmb<A0>());
#endif
        //log(2.0) in double is 6.931471805599453e-01
        //double(0.693359375f)+double(-0.00021219444f)  is  6.931471805600000e-01 at 1.0e-14 of log(2.0)
        // let us call Log_2hi 0.693359375f anf Log_2lo -0.00021219444f
        // We use thi to correct the sum where this could matter a lot
        // log(a0) = fe*Log_2hi+ (0.5f*x*x +(fe*Log_2lo+y))
        // These operations are order dependent: the parentheses do matter
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = bs::fma(fe, Log_2lo<A0>(), y);
        y = bs::fma(Mhalf<A0>(), x2, y);
#ifdef BOOST_SIMD_NO_DENORMALS
        return finalize(a0, bs::fma(Log_2hi<A0>(), fe, x+y));
#else
        return finalize(a0, bs::fma(Log_2hi<A0>(), fe, x+y+t));
#endif
      }

      static inline A0 log2(const A0& a0) BOOST_NOEXCEPT
      {
        A0 z =  a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = lt(bs::abs(z), Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 t = if_else_zero(denormal, Mlog2two2nmb<A0>());
#endif
        //here let l2em1 = log2(e)-1, the computation is done as:
        //log2(a0) = ((l2em1*x+(l2em1*(y+x*x/2)))+(y+x*x/2)))+x+fe for best results
        // once again the order is very important.
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = bs::fma(Mhalf<A0>(),x2, y);
        z = bs::fma(x,Log2_em1<A0>(),y*Log2_em1<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
        return finalize(a0, ((z+y)+x)+fe);
#else
        return finalize(a0, ((z+y)+x)+fe+t);
#endif
      }

      static inline A0 log10(const A0& a0) BOOST_NOEXCEPT
      {
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = lt(bs::abs(z), Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 t = if_else_zero(denormal, Mlog10two2nmb<A0>());
#endif
        // here there are two multiplication: log of fraction by log10(e) and base 2 exponent by log10(2)
        // and we have to split log10(e) and log10(2) in two parts to get extra precision when needed
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = bs::fma(x2, Mhalf<A0>(), y);
        z = (x+y* Log10_elo<A0>());
        z = bs::fma(y, Log10_ehi<A0>(), z);
        z = bs::fma( x, Log10_ehi<A0>(), z);
        z = bs::fma(fe, Log10_2hi<A0>(), z);
#ifdef BOOST_SIMD_NO_DENORMALS
        return finalize(a0, bs::fma(fe, Log10_2lo<A0>(), z));
#else
        return finalize(a0, bs::fma(fe, Log10_2lo<A0>(), z+t));
#endif
      }
    private:
      static inline A0 finalize(const A0& a0, const A0& y) BOOST_NOEXCEPT
      {
#ifdef BOOST_SIMD_NO_NANS
        auto test = bs::is_ltz(a0);
#else
        auto test = bs::logical_or(bs::is_ltz(a0), bs::is_nan(a0));
#endif
        A0 y1 = bs::if_nan_else(test, y);
#ifndef BOOST_SIMD_NO_INFINITIES
        y1 = if_else(bs::is_equal(a0, bs::Inf<A0>()), a0, y1);
#endif
        return if_else(is_eqz(a0), bs::Minf<A0>(), y1);
      }
    };
  }
} }

#endif


#pragma once

#include <Eigen/Dense>

namespace quadcopter_ompl {
class Quadcopter {

    public:
        Quadcopter();
        //Quadcopter(/*TODO*/);
        ~Quadcopter();
        void step(/*TODO*/);

    protected:
        void thetadot2omega(const Eigen::Vector3d&);
        void omega2thetadot(const Eigen::Vector3d&);
        void angular_acceleration(const Eigen::Vector4d&);
        void rotation(const Eigen::Vector3d&);
        void thrust(const Eigen::Vector4d&);
        void torques(const Eigen::Vector4d&);
        void acceleration(const Eigen::Vector4d&, const Eigen::Vector3d&);

    private:
        // Quadcopter properties
        // arm length, mass, thrust coefficient
        double L, m, k;
        // NOTE physics (might need to remove them or apply them to ODE)
        // gravity, drag coefficient, global drag coefficient
        double g, b, kd;
        Eigen::Vector3d a;
        Eigen::Vector3d x;
        Eigen::RowVector3d T;
        Eigen::RowVector3d Fd;
        Eigen::Vector3d tau;
        Eigen::Vector3d xdot;
        Eigen::Vector3d xdot_;
        Eigen::Vector3d theta;
        Eigen::Vector3d thetadot;
        Eigen::Vector3d omega;
        Eigen::Vector3d omegadot;
        Eigen::Matrix3d R;
        Eigen::DiagonalMatrix<double,3,3> I;
        // Eigen::Matrix3d I;

};
}