#include <memory>

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

#include <aikido/constraint/DifferentiableIntersection.hpp>
#include <aikido/constraint/dart/TSR.hpp>
#include <aikido/statespace/Rn.hpp>

#include "PolynomialConstraint.hpp"

using aikido::constraint::DifferentiableIntersection;
using aikido::constraint::DifferentiablePtr;
using aikido::constraint::dart::TSR;

using aikido::statespace::R1;
using aikido::statespace::StateSpace;
using aikido::statespace::StateSpacePtr;

TEST(DifferentiableIntersection, InvalidConstructor)
{
  std::vector<DifferentiablePtr> constraints;
  std::shared_ptr<R1> rvss(new R1());

  // empty constraints
  EXPECT_THROW(
      DifferentiableIntersection(constraints, rvss), std::invalid_argument);

  // null statespace
  StateSpacePtr space;
  constraints.push_back(std::make_shared<PolynomialConstraint<1>>(
      Eigen::Vector3d(1, 2, 3), rvss));
  EXPECT_THROW(
      DifferentiableIntersection(constraints, space), std::invalid_argument);

  // constraints have different space
  constraints.push_back(std::make_shared<PolynomialConstraint<1>>(
      Eigen::Vector3d(1, 2, 3), rvss));
  constraints.push_back(dart::common::make_aligned_shared<TSR>());
  EXPECT_THROW(
      DifferentiableIntersection(constraints, rvss), std::invalid_argument);
}

TEST(DifferentiableIntersection, getValue)
{
  std::vector<DifferentiablePtr> constraints;
  std::shared_ptr<R1> rvss(new R1());

  // constraint1: 1 + 2x + 3x^2
  constraints.push_back(std::make_shared<PolynomialConstraint<1>>(
      Eigen::Vector3d(1, 2, 3), rvss));

  // constraint2: 4 + 5x
  constraints.push_back(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector2d(4, 5), rvss));

  Eigen::VectorXd v(1);
  v(0) = -2;

  auto s1 = rvss->createState();
  s1.setValue(v);

  DifferentiableIntersection stacked(constraints, rvss);

  Eigen::Vector2d expected(9, -6);
  Eigen::VectorXd out;
  stacked.getValue(s1, out);

  EXPECT_TRUE(out.isApprox(expected));
}

TEST(DifferentiableIntersection, getJacobian)
{
  std::vector<DifferentiablePtr> constraints;
  std::shared_ptr<R1> rvss(new R1());

  // constraint1: 1 + 2x + 3x^2
  constraints.push_back(std::make_shared<PolynomialConstraint<1>>(
      Eigen::Vector3d(1, 2, 3), rvss));

  // constraint2: 4 + 5x
  constraints.push_back(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector2d(4, 5), rvss));

  Eigen::VectorXd v(1);
  v(0) = -2;

  auto s1 = rvss->createState();
  s1.setValue(v);

  DifferentiableIntersection stacked(constraints, rvss);
  Eigen::MatrixXd expected(2, 1);
  expected(0, 0) = -10;
  expected(1, 0) = 5;

  Eigen::MatrixXd out;
  stacked.getJacobian(s1, out);

  EXPECT_TRUE(out.isApprox(expected));
}

/// Compare returned values with getValue and getJacobian
TEST(DifferentiableIntersection, GetValueAndJacobianMatchValueAndJacobian)
{
  std::vector<DifferentiablePtr> constraints;
  std::shared_ptr<R1> rvss(new R1());

  // constraint1: 1 + 2x + 3x^2
  constraints.push_back(std::make_shared<PolynomialConstraint<1>>(
      Eigen::Vector3d(1, 2, 3), rvss));

  // constraint2: 4 + 5x
  constraints.push_back(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector2d(4, 5), rvss));

  auto s1 = rvss->createState();
  Eigen::VectorXd v(1);
  v(0) = -2;
  s1.setValue(v);

  DifferentiableIntersection stacked(constraints, rvss);
  Eigen::VectorXd val;
  Eigen::MatrixXd jac;
  stacked.getValueAndJacobian(s1, val, jac);

  Eigen::VectorXd value;
  Eigen::MatrixXd jacobian;

  stacked.getValue(s1, value);
  stacked.getJacobian(s1, jacobian);

  EXPECT_TRUE(val.isApprox(value));
  EXPECT_TRUE(jac.isApprox(jacobian));
}

TEST(DifferentiableIntersection, GetConstraintTypes)
{
  std::vector<DifferentiablePtr> constraints;
  std::shared_ptr<R1> rvss(new R1());

  // constraint1: 1 + 2x + 3x^2
  constraints.push_back(std::make_shared<PolynomialConstraint<1>>(
      Eigen::Vector3d(1, 2, 3), rvss));

  // constraint2: 4 + 5x
  constraints.push_back(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector2d(4, 5), rvss));

  DifferentiableIntersection stacked(constraints, rvss);
  auto constraintTypes = stacked.getConstraintTypes();

  // Check the size of ConstraintTypes
  EXPECT_EQ(constraintTypes.size(), 2);

  // Check the types
  for (auto cType : constraintTypes)
  {
    EXPECT_EQ(cType, aikido::constraint::ConstraintType::EQUALITY);
  }
}

TEST(DifferentiableIntersection, GetStateSpace)
{
  std::vector<DifferentiablePtr> constraints;
  std::shared_ptr<R1> rvss(new R1());

  // constraint1: 1 + 2x + 3x^2
  constraints.push_back(std::make_shared<PolynomialConstraint<1>>(
      Eigen::Vector3d(1, 2, 3), rvss));

  // constraint2: 4 + 5x
  constraints.push_back(
      std::make_shared<PolynomialConstraint<1>>(Eigen::Vector2d(4, 5), rvss));

  DifferentiableIntersection stacked(constraints, rvss);
  auto space = stacked.getStateSpace();

  EXPECT_EQ(space, rvss);
}


#include "catch.hpp"

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <SmurffCpp/IO/TensorIO.h>
#include <SmurffCpp/Utils/TensorUtils.h>
#include <SmurffCpp/Utils/MatrixUtils.h>

using namespace smurff;

static NoiseConfig fixed_ncfg(NoiseTypes::fixed);

TEST_CASE("tensor_io/read_dense_float64_bin | tensor_io/write_dense_float64_bin")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<double> tensorConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };
   std::shared_ptr<TensorConfig> tensorConfig(new TensorConfig(std::move(tensorConfigDims), std::move(tensorConfigValues), fixed_ncfg));

   std::stringstream matrixConfigStream;
   tensor_io::write_dense_float64_bin(matrixConfigStream, tensorConfig);

   std::shared_ptr<TensorConfig> actualTensorConfig = tensor_io::read_dense_float64_bin(matrixConfigStream);
   MatrixConfig actualMatrixConfig = tensor_utils::tensor_to_matrix(*actualTensorConfig);

   Eigen::MatrixXd actualMatrix0 = tensor_utils::dense_to_eigen(*actualTensorConfig);
   Eigen::MatrixXd actualMatrix1 = matrix_utils::dense_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("tensor_io/read_sparse_float64_bin | tensor_io/write_sparse_float64_bin")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<std::uint32_t> tensorConfigColumns = { 0, 0, 0, 0, 2, 2, 2, 2, 0, 1, 2, 3, 0, 1, 2, 3 };
   std::vector<double> tensorConfigValues = { 1, 2, 3, 4, 9, 10, 11, 12 };
   std::shared_ptr<TensorConfig> tensorConfig(new TensorConfig(std::move(tensorConfigDims), std::move(tensorConfigColumns), std::move(tensorConfigValues), fixed_ncfg, false));

   std::stringstream tensorStream;
   tensor_io::write_sparse_float64_bin(tensorStream, tensorConfig);

   std::shared_ptr<TensorConfig> actualTensorConfig = tensor_io::read_sparse_float64_bin(tensorStream, false);
   MatrixConfig actualMatrixConfig = tensor_utils::tensor_to_matrix(*actualTensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(*actualTensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("tensor_io/read_sparse_binary_bin | tensor_io/write_sparse_binary_bin")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<std::uint32_t> tensorConfigColumns = { 0, 0, 0, 0, 2, 2, 2, 2, 0, 1, 2, 3, 0, 1, 2, 3 };
   std::shared_ptr<TensorConfig> tensorConfig(new TensorConfig(std::move(tensorConfigDims), std::move(tensorConfigColumns), fixed_ncfg, false));

   std::stringstream tensorStream;
   tensor_io::write_sparse_binary_bin(tensorStream, tensorConfig);

   std::shared_ptr<TensorConfig> actualTensorConfig = tensor_io::read_sparse_binary_bin(tensorStream, false);
   MatrixConfig actualMatrixConfig = tensor_utils::tensor_to_matrix(*actualTensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(*actualTensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 1));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("tensor_io/read_dense_float64_csv | tensor_io/write_dense_float64_csv")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<double> tensorConfigValues = { 1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12 };
   std::shared_ptr<TensorConfig> tensorConfig(new TensorConfig(std::move(tensorConfigDims), std::move(tensorConfigValues), fixed_ncfg));

   std::stringstream matrixConfigStream;
   tensor_io::write_dense_float64_csv(matrixConfigStream, tensorConfig);

   std::shared_ptr<TensorConfig> actualTensorConfig = tensor_io::read_dense_float64_csv(matrixConfigStream);
   MatrixConfig actualMatrixConfig = tensor_utils::tensor_to_matrix(*actualTensorConfig);

   Eigen::MatrixXd actualMatrix0 = tensor_utils::dense_to_eigen(*actualTensorConfig);
   Eigen::MatrixXd actualMatrix1 = matrix_utils::dense_to_eigen(actualMatrixConfig);

   Eigen::MatrixXd expectedMatrix(3, 4);
   expectedMatrix << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}

TEST_CASE("tensor_io/read_sparse_float64_tns | tensor_io/write_sparse_float64_tns")
{
   std::vector<std::uint64_t> tensorConfigDims = { 3, 4 };
   std::vector<std::uint32_t> tensorConfigColumns = { 0, 0, 0, 0, 2, 2, 2, 2, 0, 1, 2, 3, 0, 1, 2, 3 };
   std::vector<double> tensorConfigValues = { 1, 2, 3, 4, 9, 10, 11, 12 };
   std::shared_ptr<TensorConfig> tensorConfig(new TensorConfig(std::move(tensorConfigDims), std::move(tensorConfigColumns), std::move(tensorConfigValues), fixed_ncfg, false));

   std::stringstream tensorStream;
   tensor_io::write_sparse_float64_tns(tensorStream, tensorConfig);

   std::shared_ptr<TensorConfig> actualTensorConfig = tensor_io::read_sparse_float64_tns(tensorStream, false);
   MatrixConfig actualMatrixConfig = tensor_utils::tensor_to_matrix(*actualTensorConfig);

   Eigen::SparseMatrix<double> actualMatrix0 = tensor_utils::sparse_to_eigen(*actualTensorConfig);
   Eigen::SparseMatrix<double> actualMatrix1 = matrix_utils::sparse_to_eigen(actualMatrixConfig);

   Eigen::SparseMatrix<double> expectedMatrix(3, 4);
   std::vector<Eigen::Triplet<double> > expectedMatrixTriplets;
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 0, 1));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 1, 2));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 2, 3));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(0, 3, 4));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 0, 9));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 1, 10));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 2, 11));
   expectedMatrixTriplets.push_back(Eigen::Triplet<double>(2, 3, 12));
   expectedMatrix.setFromTriplets(expectedMatrixTriplets.begin(), expectedMatrixTriplets.end());

   REQUIRE(matrix_utils::equals(actualMatrix0, expectedMatrix));
   REQUIRE(matrix_utils::equals(actualMatrix1, expectedMatrix));
}


// -*-C++-*-
// bddmain.cpp
// CBdd class implementation

#include "core/utils.h"
#include "core/bddmain.h"

#include <boost/dynamic_bitset.hpp>
#include <deque>

bdd_manager CBdd::manager=0;
bdd* CBdd::vars=0;
int CBdd::length=0;

void CBdd::Init(int len, int props, ...)
{
	va_list ap;
	va_start(ap, props);
	va_end(ap);

	length = len;

	vars = new bdd[len+1];
	manager = bdd_init();

	bdd_node_limit(manager, 10000000);

	if(mtbdd_one_data(manager, -1l, -1l)!=0)    // mtbdd one
	{
		throw new CDesertException("mtbdd_one_data fails: other terminal nodes already exist");
	}

	vars[0]=bdd_new_var_first(manager);
	for(int i=1; i<=length; i++)
		vars[i]=bdd_new_var_after(manager, vars[i-1]);
}

void CBdd::Finit(void)
{
	for (int i=0; i<length; i++)
		bdd_free(manager, vars[i]);
	bdd_quit(manager);
	delete[] vars;

	manager=0;
	length=0;
	vars=0;
}

CBdd CBdd::Encode(int encVal, int startVar, int encLen)
{
	MANAGER_CHECK("CBdd::Encode()");
	bdd ret=bdd_one(manager);
	int i, n;
	for(i=startVar+encLen-1, n=0; n < encLen; i--, n++)
	{
		int bit = encVal & 0x1;
		encVal = encVal >> 1;
		ret = bit ? bdd_and(manager, ret, vars[i] ) : bdd_and(manager, ret, bdd_not(manager, vars[i]) );
	}
	return CBdd(ret);
}

CBdd CBdd::Encode(const boost::dynamic_bitset<>& enc, int begin_var, int num_vars)
{
	MANAGER_CHECK("CBdd::Encode()");
	bdd ret = bdd_one(manager);
	int i, n;
	for (i = begin_var, n = 0; n < num_vars; i++, n++)
	{
		if (enc[n] == true) ret = bdd_and(manager, ret, vars[i]);
		if (enc[n] == false) ret = bdd_and(manager, ret, bdd_not(manager, vars[i]));
	}
	return CBdd(ret);
}

CBdd CBdd::Encode(int *enc, int begin_var, int num_vars)
{
	MANAGER_CHECK("CBdd::Encode()");
	bdd ret=bdd_one(manager);
	int i, n;
	for(i=begin_var, n=0; n < num_vars; i++, n++)
	{
		if(enc[n] == 1) ret = bdd_and(manager, ret, vars[i] );
		if(enc[n] == 0) ret = bdd_and(manager, ret, bdd_not(manager, vars[i]) );
	}
	return CBdd(ret);
}

CBdd CBdd::Encode(CVIndex enc[], int len)
{
  MANAGER_CHECK("CBdd::Encode()");
  bdd ret=bdd_one(manager);
  for(int i=0; i < len; i++)
  {
    if (enc[i].index >= length)
    {
      Error(_T("CBddManager::m_encode"), _T("index (%d) >= length (%d)"), enc[i].index, length);
      break;
    }
    if(enc[i].val == 1) ret = bdd_and(manager, ret, vars[enc[i].index] );
    if(enc[i].val == 0) ret = bdd_and(manager, ret, bdd_not(manager, vars[enc[i].index]));
  }
  return CBdd(ret);
}

int CBdd::Satisfy(CBdd& b, std::deque<boost::dynamic_bitset<>>& encVectors, int (*callback)(void*, const boost::dynamic_bitset<>&), void* arg)
{
	/*
	The function Satisfy fills up and returns a list of bitvectors.
	The size of the list is the number of configurations.
	This size of bitvector is the number of bits in the encoding. For ex.
	for a 2-bit encoding Satisy may return something like 00, 10, 11. Thus,
	in effect it enumerates and returns all encodings that satisfy the BDD. This
	internalyy uses the bdd library function bdd_satisfy.
	*/
  bdd f = b.core;

  if (f == bdd_one(manager))
  {
    int *encVec = new int[length];
    memset(encVec, 0xff, sizeof(int)*length);
    ExpandDontCare(encVec, 0, encVectors, callback, arg);
    delete[] encVec;
  }
  else
  {
    int rows=0;
    int **mat = new int*[BDD_MAX_PATHS];
    bdd_sat_f_mat(manager, f, 0, length-1, mat, &rows);
	if (rows > BDD_MAX_PATHS) {
		for (int i = 0; i < BDD_MAX_PATHS; i++)
		{
			free(mat[i]);
		}
		delete[] mat;
		return DESIGN_SPACE_TOO_LARGE;
	}
    if (rows > 0)
    {
      for (int i=0; i<rows; i++)
      {
        ExpandDontCare(mat[i], 0, encVectors, callback, arg);
        // mat[i] is allocated in bdd_sat_f_mat
        free(mat[i]);
      }
    }

    delete[] mat;
  }

  return encVectors.size();
}

void CBdd::ExpandDontCare(int *enc, int cur, std::deque<boost::dynamic_bitset<>>& encVectors, int(*callback)(void*, const boost::dynamic_bitset<>&), void* arg)
{
	/*
	enumerate the  dont care's
	*/
  if (cur == length)
  {
	  boost::dynamic_bitset<> bs(length);
	  for (int i = 0; i < length; i++)
	  {
#ifdef _DEBUG
		  if (enc[i] != 0 && enc[i] != 1)
			  throw std::runtime_error("Internal error 162");
#endif
		  bs.set(i, (bool)enc[i]);
	  }
	  if (callback == NULL || (*callback)(arg, bs)) {
		encVectors.emplace_back(std::move(bs));
	  }
  } else if (enc[cur] == -1) {
    enc[cur] = 0;
    ExpandDontCare(enc, cur + 1, encVectors, callback, arg);
    enc[cur] = 1;
    ExpandDontCare(enc, cur + 1, encVectors, callback, arg);
    enc[cur] = -1;
  } else {
    ExpandDontCare(enc, cur + 1, encVectors, callback, arg);
  }
}


/*
 * Copyright 2018 Autoware Foundation. All rights reserved.
 *
 * 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 <stack>

#include <boost/optional.hpp>

#include <autoware_planning_msgs/PathPoint.h>
#include <autoware_planning_msgs/TrajectoryPoint.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/MapMetaData.h>

// #include <ros/console.h>;

#include <tf2/utils.h>

#include <Eigen/Core>

#include <spline_interpolation/spline_interpolation.h>

#include "obstacle_avoidance_planner/eb_path_optimizer.h"
#include "obstacle_avoidance_planner/mpt_optimizer.h"
#include "obstacle_avoidance_planner/util.h"

namespace util
{
template <typename T>
geometry_msgs::Point transformToRelativeCoordinate2D(
  const T & point, const geometry_msgs::Pose & origin)
{
  geometry_msgs::Transform origin_coord2point;
  origin_coord2point.translation.x = point.x;
  origin_coord2point.translation.y = point.y;
  tf2::Transform tf_origin_coord2point;
  tf2::fromMsg(origin_coord2point, tf_origin_coord2point);

  geometry_msgs::Transform origin_coord2origin;
  origin_coord2origin.translation.x = origin.position.x;
  origin_coord2origin.translation.y = origin.position.y;
  origin_coord2origin.rotation = origin.orientation;
  tf2::Transform tf_origin_coord2origin;
  tf2::fromMsg(origin_coord2origin, tf_origin_coord2origin);

  tf2::Transform tf_origin2origin_coord = tf_origin_coord2origin.inverse() * tf_origin_coord2point;
  geometry_msgs::Pose rel_pose;
  tf2::toMsg(tf_origin2origin_coord, rel_pose);
  geometry_msgs::Point relative_p = rel_pose.position;
  return relative_p;
}
template geometry_msgs::Point transformToRelativeCoordinate2D<geometry_msgs::Point>(
  const geometry_msgs::Point &, const geometry_msgs::Pose & origin);
template geometry_msgs::Point transformToRelativeCoordinate2D<geometry_msgs::Point32>(
  const geometry_msgs::Point32 &, const geometry_msgs::Pose & origin);

geometry_msgs::Point transformToAbsoluteCoordinate2D(
  const geometry_msgs::Point & point, const geometry_msgs::Pose & origin)
{
  geometry_msgs::Transform origin2point;
  origin2point.translation.x = point.x;
  origin2point.translation.y = point.y;
  tf2::Transform tf_origin2point;
  tf2::fromMsg(origin2point, tf_origin2point);

  geometry_msgs::Transform origin_coord2origin;
  origin_coord2origin.translation.x = origin.position.x;
  origin_coord2origin.translation.y = origin.position.y;
  origin_coord2origin.rotation = origin.orientation;
  tf2::Transform tf_origin_coord2origin;
  tf2::fromMsg(origin_coord2origin, tf_origin_coord2origin);
  tf2::Transform tf_origin_coord2point = tf_origin_coord2origin * tf_origin2point;

  geometry_msgs::Pose abs_pose;
  tf2::toMsg(tf_origin_coord2point, abs_pose);
  geometry_msgs::Point abs_p = abs_pose.position;
  return abs_p;
}

double calculate2DDistance(const geometry_msgs::Point & a, const geometry_msgs::Point & b)
{
  const double dx = a.x - b.x;
  const double dy = a.y - b.y;
  return std::sqrt(dx * dx + dy * dy);
}

double calculateSquaredDistance(const geometry_msgs::Point & a, const geometry_msgs::Point & b)
{
  const double dx = a.x - b.x;
  const double dy = a.y - b.y;
  return dx * dx + dy * dy;
}

double getYawFromPoints(const geometry_msgs::Point & a, const geometry_msgs::Point & a_root)
{
  const double dx = a.x - a_root.x;
  const double dy = a.y - a_root.y;
  return std::atan2(dy, dx);
}

double normalizeRadian(const double angle)
{
  double n_angle = std::fmod(angle, 2 * M_PI);
  n_angle = n_angle > M_PI ? n_angle - 2 * M_PI : n_angle < -M_PI ? 2 * M_PI + n_angle : n_angle;
  return n_angle;
}

geometry_msgs::Quaternion getQuaternionFromPoints(
  const geometry_msgs::Point & a, const geometry_msgs::Point & a_root)
{
  const double roll = 0;
  const double pitch = 0;
  const double yaw = util::getYawFromPoints(a, a_root);
  tf2::Quaternion quaternion;
  quaternion.setRPY(roll, pitch, yaw);
  return tf2::toMsg(quaternion);
}

template <typename T>
geometry_msgs::Point transformMapToImage(
  const T & map_point, const nav_msgs::MapMetaData & occupancy_grid_info)
{
  geometry_msgs::Point relative_p =
    transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin);
  double resolution = occupancy_grid_info.resolution;
  double map_y_height = occupancy_grid_info.height;
  double map_x_width = occupancy_grid_info.width;
  double map_x_in_image_resolution = relative_p.x / resolution;
  double map_y_in_image_resolution = relative_p.y / resolution;
  geometry_msgs::Point image_point;
  image_point.x = map_y_height - map_y_in_image_resolution;
  image_point.y = map_x_width - map_x_in_image_resolution;
  return image_point;
}
template geometry_msgs::Point transformMapToImage<geometry_msgs::Point>(
  const geometry_msgs::Point &, const nav_msgs::MapMetaData & map_info);
template geometry_msgs::Point transformMapToImage<geometry_msgs::Point32>(
  const geometry_msgs::Point32 &, const nav_msgs::MapMetaData & map_info);

boost::optional<geometry_msgs::Point> transformMapToOptionalImage(
  const geometry_msgs::Point & map_point, const nav_msgs::MapMetaData & occupancy_grid_info)
{
  geometry_msgs::Point relative_p =
    transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin);
  double resolution = occupancy_grid_info.resolution;
  double map_y_height = occupancy_grid_info.height;
  double map_x_width = occupancy_grid_info.width;
  double map_x_in_image_resolution = relative_p.x / resolution;
  double map_y_in_image_resolution = relative_p.y / resolution;
  double image_x = map_y_height - map_y_in_image_resolution;
  double image_y = map_x_width - map_x_in_image_resolution;
  if (image_x >= 0 && image_x < (int)map_y_height && image_y >= 0 && image_y < (int)map_x_width) {
    geometry_msgs::Point image_point;
    image_point.x = image_x;
    image_point.y = image_y;
    return image_point;
  } else {
    return boost::none;
  }
}

bool transformMapToImage(
  const geometry_msgs::Point & map_point, const nav_msgs::MapMetaData & occupancy_grid_info,
  geometry_msgs::Point & image_point)
{
  geometry_msgs::Point relative_p =
    transformToRelativeCoordinate2D(map_point, occupancy_grid_info.origin);
  const double map_y_height = occupancy_grid_info.height;
  const double map_x_width = occupancy_grid_info.width;
  const double scale = 1 / occupancy_grid_info.resolution;
  const double map_x_in_image_resolution = relative_p.x * scale;
  const double map_y_in_image_resolution = relative_p.y * scale;
  const double image_x = map_y_height - map_y_in_image_resolution;
  const double image_y = map_x_width - map_x_in_image_resolution;
  if (image_x >= 0 && image_x < (int)map_y_height && image_y >= 0 && image_y < (int)map_x_width) {
    image_point.x = image_x;
    image_point.y = image_y;
    return true;
  } else {
    return false;
  }
}

bool interpolate2DPoints(
  const std::vector<double> & base_x, const std::vector<double> & base_y, const double resolution,
  std::vector<geometry_msgs::Point> & interpolated_points)
{
  if (base_x.empty() || base_y.empty()) {
    return false;
  }
  std::vector<double> base_s = calcEuclidDist(base_x, base_y);
  if (base_s.empty() || base_s.size() == 1) {
    return false;
  }
  std::vector<double> new_s;
  for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) {
    new_s.push_back(i);
  }
  spline_interpolation::SplineInterpolator spline;
  // spline::SplineInterpolate spline;
  std::vector<double> interpolated_x;
  std::vector<double> interpolated_y;

  const int num_sample = base_s.size();
  constexpr int num_sample_threshold = 10;
  if (num_sample > num_sample_threshold) {
    spline.interpolate(base_s, base_x, new_s, interpolated_x, spline_interpolation::Method::PCG);
    spline.interpolate(base_s, base_y, new_s, interpolated_y, spline_interpolation::Method::PCG);
  } else {
    spline.interpolate(base_s, base_x, new_s, interpolated_x, spline_interpolation::Method::SOR);
    spline.interpolate(base_s, base_y, new_s, interpolated_y, spline_interpolation::Method::SOR);
  }

  for (size_t i = 0; i < interpolated_x.size(); i++) {
    if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
      return false;
    }
  }
  for (size_t i = 0; i < interpolated_x.size(); i++) {
    geometry_msgs::Point point;
    point.x = interpolated_x[i];
    point.y = interpolated_y[i];
    interpolated_points.push_back(point);
  }
}

std::vector<geometry_msgs::Point> getInterpolatedPoints(
  const std::vector<geometry_msgs::Pose> & first_points,
  const std::vector<geometry_msgs::Pose> & second_points, const double delta_arc_length)
{
  std::vector<double> tmp_x;
  std::vector<double> tmp_y;
  std::vector<geometry_msgs::Point> concat_points;
  for (const auto point : first_points) {
    concat_points.push_back(point.position);
  }
  for (const auto & point : second_points) {
    concat_points.push_back(point.position);
  }

  for (int i = 0; i < concat_points.size(); i++) {
    if (i > 0) {
      if (
        std::fabs(concat_points[i].x - concat_points[i - 1].x) < 1e-6 &&
        std::fabs(concat_points[i].y - concat_points[i - 1].y) < 1e-6) {
        continue;
      }
    }
    tmp_x.push_back(concat_points[i].x);
    tmp_y.push_back(concat_points[i].y);
  }
  std::vector<geometry_msgs::Point> interpolated_points;
  util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points);
  return interpolated_points;
}

std::vector<geometry_msgs::Point> getInterpolatedPoints(
  const std::vector<geometry_msgs::Pose> & points, const double delta_arc_length)
{
  std::vector<double> tmp_x;
  std::vector<double> tmp_y;
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      if (
        std::fabs(points[i].position.x - points[i - 1].position.x) < 1e-6 &&
        std::fabs(points[i].position.y - points[i - 1].position.y) < 1e-6) {
        continue;
      }
    }
    tmp_x.push_back(points[i].position.x);
    tmp_y.push_back(points[i].position.y);
  }
  std::vector<geometry_msgs::Point> interpolated_points;
  util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points);
  return interpolated_points;
}

std::vector<geometry_msgs::Point> getInterpolatedPoints(
  const std::vector<ReferencePoint> & points, const double delta_arc_length)
{
  std::vector<double> tmp_x;
  std::vector<double> tmp_y;
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      if (
        std::fabs(points[i].p.x - points[i - 1].p.x) < 1e-6 &&
        std::fabs(points[i].p.y - points[i - 1].p.y) < 1e-6) {
        continue;
      }
    }
    tmp_x.push_back(points[i].p.x);
    tmp_y.push_back(points[i].p.y);
  }
  std::vector<geometry_msgs::Point> interpolated_points;
  util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points);
  return interpolated_points;
}

template <typename T>
std::vector<geometry_msgs::Point> getInterpolatedPoints(
  const T & points, const double delta_arc_length)
{
  std::vector<double> tmp_x;
  std::vector<double> tmp_y;
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      if (
        std::fabs(points[i].pose.position.x - points[i - 1].pose.position.x) < 1e-6 &&
        std::fabs(points[i].pose.position.y - points[i - 1].pose.position.y) < 1e-6) {
        continue;
      }
    }
    tmp_x.push_back(points[i].pose.position.x);
    tmp_y.push_back(points[i].pose.position.y);
  }
  std::vector<geometry_msgs::Point> interpolated_points;
  util::interpolate2DPoints(tmp_x, tmp_y, delta_arc_length, interpolated_points);
  return interpolated_points;
}
template std::vector<geometry_msgs::Point>
getInterpolatedPoints<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &, const double);
template std::vector<geometry_msgs::Point>
getInterpolatedPoints<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> &, const double);

template <typename T>
int getNearestIdx(
  const T & points, const geometry_msgs::Pose & pose, const int default_idx,
  const double delta_yaw_threshold)
{
  double min_dist = std::numeric_limits<double>::max();
  int nearest_idx = default_idx;
  const double point_yaw = tf2::getYaw(pose.orientation);
  for (int i = 0; i < points.size(); i++) {
    const double dist = calculateSquaredDistance(points[i].pose.position, pose.position);
    const double points_yaw = tf2::getYaw(points[i].pose.orientation);
    const double diff_yaw = points_yaw - point_yaw;
    const double norm_diff_yaw = normalizeRadian(diff_yaw);
    if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
      min_dist = dist;
      nearest_idx = i;
    }
  }
  return nearest_idx;
}
template int getNearestIdx<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &, const geometry_msgs::Pose &,
  const int, const double);
template int getNearestIdx<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> &, const geometry_msgs::Pose &, const int,
  const double);

int getNearestIdx(
  const std::vector<geometry_msgs::Point> & points, const geometry_msgs::Pose & pose,
  const int default_idx, const double delta_yaw_threshold, const double delta_dist_threshold)
{
  int nearest_idx = default_idx;
  double min_dist = std::numeric_limits<double>::max();
  const double point_yaw = tf2::getYaw(pose.orientation);
  for (int i = 0; i < points.size(); i++) {
    const double dist = calculateSquaredDistance(points[i], pose.position);
    double points_yaw = 0;
    if (i > 0) {
      const double dx = points[i].x - points[i - 1].x;
      const double dy = points[i].y - points[i - 1].y;
      points_yaw = std::atan2(dy, dx);
    } else if (i == 0 && points.size() > 1) {
      const double dx = points[i + 1].x - points[i].x;
      const double dy = points[i + 1].y - points[i].y;
      points_yaw = std::atan2(dy, dx);
    }
    const double diff_yaw = points_yaw - point_yaw;
    const double norm_diff_yaw = normalizeRadian(diff_yaw);
    if (
      dist < min_dist && dist < delta_dist_threshold &&
      std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
      min_dist = dist;
      nearest_idx = i;
    }
  }
  return nearest_idx;
}

int getNearestIdxOverPoint(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & points,
  const geometry_msgs::Pose & pose, const int default_idx, const double delta_yaw_threshold)
{
  double min_dist = std::numeric_limits<double>::max();
  const double point_yaw = tf2::getYaw(pose.orientation);
  const double pose_dx = std::cos(point_yaw);
  const double pose_dy = std::sin(point_yaw);
  int nearest_idx = 0;
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      const double dist = util::calculateSquaredDistance(points[i].pose.position, pose.position);
      const double points_yaw =
        getYawFromPoints(points[i].pose.position, points[i - 1].pose.position);
      const double diff_yaw = points_yaw - point_yaw;
      const double norm_diff_yaw = normalizeRadian(diff_yaw);
      const double dx = points[i].pose.position.x - pose.position.x;
      const double dy = points[i].pose.position.y - pose.position.y;
      const double ip = dx * pose_dx + dy * pose_dy;
      if (dist < min_dist && ip > 0 && std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
        min_dist = dist;
        nearest_idx = i;
      }
    }
  }
  return nearest_idx;
}

int getNearestIdx(
  const std::vector<geometry_msgs::Point> & points, const geometry_msgs::Pose & pose,
  const int default_idx, const double delta_yaw_threshold)
{
  double min_dist = std::numeric_limits<double>::max();
  int nearest_idx = default_idx;
  const double point_yaw = tf2::getYaw(pose.orientation);
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      const double dist = calculateSquaredDistance(points[i], pose.position);
      const double points_yaw = getYawFromPoints(points[i], points[i - 1]);
      const double diff_yaw = points_yaw - point_yaw;
      const double norm_diff_yaw = normalizeRadian(diff_yaw);
      if (dist < min_dist && std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
        min_dist = dist;
        nearest_idx = i;
      }
    }
  }
  return nearest_idx;
}

template <typename T>
int getNearestIdx(const T & points, const geometry_msgs::Point & point, const int default_idx)
{
  double min_dist = std::numeric_limits<double>::max();
  int nearest_idx = default_idx;
  for (int i = 0; i < points.size(); i++) {
    if (i > 0) {
      const double dist = calculateSquaredDistance(points[i - 1].pose.position, point);
      const double points_dx = points[i].pose.position.x - points[i - 1].pose.position.x;
      const double points_dy = points[i].pose.position.y - points[i - 1].pose.position.y;
      const double points2pose_dx = point.x - points[i - 1].pose.position.x;
      const double points2pose_dy = point.y - points[i - 1].pose.position.y;
      const double ip = points_dx * points2pose_dx + points_dy * points2pose_dy;
      if (ip < 0) {
        return nearest_idx;
      }
      if (dist < min_dist && ip > 0) {
        min_dist = dist;
        nearest_idx = i - 1;
      }
    }
  }
  return nearest_idx;
}
template int getNearestIdx<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & points,
  const geometry_msgs::Point & point, const int default_idx);
template int getNearestIdx<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> & points, const geometry_msgs::Point & point,
  const int default_idx);

int getNearestIdx(
  const std::vector<geometry_msgs::Point> & points, const geometry_msgs::Point & point)
{
  int nearest_idx = 0;
  double min_dist = std::numeric_limits<double>::max();
  for (int i = 0; i < points.size(); i++) {
    const double dist = calculateSquaredDistance(points[i], point);
    if (dist < min_dist) {
      min_dist = dist;
      nearest_idx = i;
    }
  }
  return nearest_idx;
}

template <typename T>
int getNearestIdx(const T & points, const geometry_msgs::Point & point)
{
  int nearest_idx = 0;
  double min_dist = std::numeric_limits<double>::max();
  for (int i = 0; i < points.size(); i++) {
    const double dist = calculateSquaredDistance(points[i].pose.position, point);
    if (dist < min_dist) {
      min_dist = dist;
      nearest_idx = i;
    }
  }
  return nearest_idx;
}
template int getNearestIdx<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &, const geometry_msgs::Point &);
template int getNearestIdx<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> &, const geometry_msgs::Point &);

int getNearestIdx(
  const std::vector<ReferencePoint> & points, const double target_s, const int begin_idx)
{
  double nearest_delta_s = std::numeric_limits<double>::max();
  int nearest_idx = begin_idx;
  for (int i = begin_idx; i < points.size(); i++) {
    double diff = std::fabs(target_s - points[i].s);
    if (diff < nearest_delta_s) {
      nearest_delta_s = diff;
      nearest_idx = i;
    }
  }
  return nearest_idx;
}

template <typename T>
int getNearestPointIdx(const T & points, const geometry_msgs::Point & point)
{
  int nearest_idx = 0;
  double min_dist = std::numeric_limits<double>::max();
  for (int i = 0; i < points.size(); i++) {
    const double dist = calculateSquaredDistance(points[i].p, point);
    if (dist < min_dist) {
      min_dist = dist;
      nearest_idx = i;
    }
  }
  return nearest_idx;
}
template int getNearestPointIdx<std::vector<ReferencePoint>>(
  const std::vector<ReferencePoint> &, const geometry_msgs::Point &);
template int getNearestPointIdx<std::vector<Footprint>>(
  const std::vector<Footprint> &, const geometry_msgs::Point &);

std::vector<autoware_planning_msgs::TrajectoryPoint> convertPathToTrajectory(
  const std::vector<autoware_planning_msgs::PathPoint> & path_points)
{
  std::vector<autoware_planning_msgs::TrajectoryPoint> traj_points;
  for (const auto & point : path_points) {
    autoware_planning_msgs::TrajectoryPoint tmp_point;
    tmp_point.pose = point.pose;
    tmp_point.twist = point.twist;
    traj_points.push_back(tmp_point);
  }
  return traj_points;
}

std::vector<autoware_planning_msgs::TrajectoryPoint> convertPointsToTrajectoryPoinsWithYaw(
  const std::vector<geometry_msgs::Point> & points)
{
  std::vector<autoware_planning_msgs::TrajectoryPoint> traj_points;
  for (size_t i = 0; i < points.size(); i++) {
    autoware_planning_msgs::TrajectoryPoint traj_point;
    traj_point.pose.position = points[i];
    double yaw = 0;
    if (i > 0) {
      const double dx = points[i].x - points[i - 1].x;
      const double dy = points[i].y - points[i - 1].y;
      yaw = std::atan2(dy, dx);
    } else if (i == 0 && points.size() > 1) {
      const double dx = points[i + 1].x - points[i].x;
      const double dy = points[i + 1].y - points[i].y;
      yaw = std::atan2(dy, dx);
    }
    const double roll = 0;
    const double pitch = 0;
    tf2::Quaternion quaternion;
    quaternion.setRPY(roll, pitch, yaw);
    traj_point.pose.orientation = tf2::toMsg(quaternion);
    traj_points.push_back(traj_point);
  }
  return traj_points;
}

std::vector<autoware_planning_msgs::TrajectoryPoint> fillTrajectoryWithVelocity(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & traj_points, const double velocity)
{
  std::vector<autoware_planning_msgs::TrajectoryPoint> traj_with_velo;
  for (const auto & traj_point : traj_points) {
    auto tmp_point = traj_point;
    tmp_point.twist.linear.x = velocity;
    traj_with_velo.push_back(tmp_point);
  }
  return traj_with_velo;
}

template <typename T>
std::vector<autoware_planning_msgs::TrajectoryPoint> alignVelocityWithPoints(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & base_traj_points, const T & points,
  const int zero_velocity_traj_idx, const int max_skip_comparison_idx)
{
  auto traj_points = base_traj_points;
  int prev_begin_idx = 0;
  for (int i = 0; i < traj_points.size(); i++) {
    const auto first = points.begin() + prev_begin_idx;
    const auto last = points.end();
    const T truncated_points(first, last);
    const int default_idx = 0;
    const int nearest_idx =
      util::getNearestIdx(truncated_points, traj_points[i].pose.position, default_idx);
    traj_points[i].pose.position.z = truncated_points[nearest_idx].pose.position.z;
    if (i <= max_skip_comparison_idx) {
      traj_points[i].twist.linear.x = truncated_points[nearest_idx].twist.linear.x;
    } else {
      traj_points[i].twist.linear.x =
        std::fmin(truncated_points[nearest_idx].twist.linear.x, traj_points[i].twist.linear.x);
    }
    if (i >= zero_velocity_traj_idx) {
      traj_points[i].twist.linear.x = 0;
    } else if (truncated_points[nearest_idx].twist.linear.x < 1e-6) {
      if (i > 0) {
        traj_points[i].twist.linear.x = traj_points[i - 1].twist.linear.x;
      } else {
        traj_points[i].twist.linear.x = points.front().twist.linear.x;
      }
    }
    prev_begin_idx += nearest_idx;
  }
  return traj_points;
}
template std::vector<autoware_planning_msgs::TrajectoryPoint>
alignVelocityWithPoints<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &,
  const std::vector<autoware_planning_msgs::PathPoint> &, const int, const int);
template std::vector<autoware_planning_msgs::TrajectoryPoint>
alignVelocityWithPoints<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &,
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &, const int, const int);

std::vector<std::vector<int>> getHistogramTable(const std::vector<std::vector<int>> & input)
{
  std::vector<std::vector<int>> histogram_table = input;
  for (int i = 0; i < input.size(); i++) {
    for (int j = 0; j < input[i].size(); j++) {
      if (input[i][j]) {
        histogram_table[i][j] = 0;
      } else {
        histogram_table[i][j] = (i > 0) ? histogram_table[i - 1][j] + 1 : 1;
      }
    }
  }
  return histogram_table;
}

struct HistogramBin
{
  int height;
  int variable_pos;
  int original_pos;
};

Rectangle getLargestRectancleInRow(
  const std::vector<int> & histo, const int curret_row, const int row_size)
{
  std::vector<int> search_histo = histo;
  search_histo.push_back(0);
  std::stack<HistogramBin> stack;
  Rectangle largest_rect;
  for (int i = 0; i < search_histo.size(); i++) {
    HistogramBin bin;
    bin.height = search_histo[i];
    bin.variable_pos = i;
    bin.original_pos = i;
    if (stack.empty()) {
      stack.push(bin);
    } else {
      if (stack.top().height < bin.height) {
        stack.push(bin);
      } else if (stack.top().height >= bin.height) {
        int target_i = i;
        while (!stack.empty() && bin.height <= stack.top().height) {
          HistogramBin tmp_bin = stack.top();
          stack.pop();
          int area = (i - tmp_bin.variable_pos) * tmp_bin.height;
          if (area > largest_rect.area) {
            largest_rect.max_y_idx = tmp_bin.variable_pos;
            largest_rect.min_y_idx = i - 1;
            largest_rect.max_x_idx = curret_row - tmp_bin.height + 1;
            largest_rect.min_x_idx = curret_row;
            largest_rect.area = area;
          }

          target_i = tmp_bin.variable_pos;
        }
        bin.variable_pos = target_i;
        stack.push(bin);
      }
    }
  }
  return largest_rect;
}

Rectangle getLargestRectangle(const std::vector<std::vector<int>> & input)
{
  std::vector<std::vector<int>> histogram_table = getHistogramTable(input);
  Rectangle largest_rectangle;
  for (int i = 0; i < histogram_table.size(); i++) {
    Rectangle rect = getLargestRectancleInRow(histogram_table[i], i, input.size());
    if (rect.area > largest_rectangle.area) {
      largest_rectangle = rect;
    }
  }
  return largest_rectangle;
}

boost::optional<geometry_msgs::Point> getLastExtendedPoint(
  const autoware_planning_msgs::PathPoint & path_point, const geometry_msgs::Pose & pose,
  const double delta_yaw_threshold, const double delta_dist_threshold)
{
  const double dist = calculate2DDistance(path_point.pose.position, pose.position);
  const double diff_yaw = tf2::getYaw(path_point.pose.orientation) - tf2::getYaw(pose.orientation);
  const double norm_diff_yaw = normalizeRadian(diff_yaw);
  if (
    dist > 1e-6 && dist < delta_dist_threshold && std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
    return path_point.pose.position;
  } else {
    return boost::none;
  }
}

boost::optional<autoware_planning_msgs::TrajectoryPoint> getLastExtendedTrajPoint(
  const autoware_planning_msgs::PathPoint & path_point, const geometry_msgs::Pose & pose,
  const double delta_yaw_threshold, const double delta_dist_threshold)
{
  const double dist = calculate2DDistance(path_point.pose.position, pose.position);
  const double diff_yaw = tf2::getYaw(path_point.pose.orientation) - tf2::getYaw(pose.orientation);
  const double norm_diff_yaw = normalizeRadian(diff_yaw);
  if (
    dist > 1e-6 && dist < delta_dist_threshold && std::fabs(norm_diff_yaw) < delta_yaw_threshold) {
    autoware_planning_msgs::TrajectoryPoint tmp_traj_p;
    tmp_traj_p.pose.position = path_point.pose.position;
    tmp_traj_p.pose.orientation =
      util::getQuaternionFromPoints(path_point.pose.position, pose.position);
    tmp_traj_p.twist = path_point.twist;
    return tmp_traj_p;
  } else {
    return boost::none;
  }
}

std::vector<Footprint> getVehicleFootprints(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> & optimzied_points,
  const VehicleParam & vehicle_param)
{
  const double baselink_to_top = vehicle_param.length - vehicle_param.rear_overhang;
  const double half_width = vehicle_param.width * 0.5;
  std::vector<double> rel_lon_offset{baselink_to_top, baselink_to_top, 0, 0};
  std::vector<double> rel_lat_offset{half_width, -half_width, -half_width, half_width};
  std::vector<Footprint> rects;
  for (int i = 0; i < optimzied_points.size(); i++) {
    // for (int i = nearest_idx; i < optimzied_points.size(); i++) {
    std::vector<geometry_msgs::Point> abs_points;
    for (int j = 0; j < rel_lon_offset.size(); j++) {
      geometry_msgs::Point rel_point;
      rel_point.x = rel_lon_offset[j];
      rel_point.y = rel_lat_offset[j];
      abs_points.push_back(
        util::transformToAbsoluteCoordinate2D(rel_point, optimzied_points[i].pose));
    }
    Footprint rect;
    rect.p = optimzied_points[i].pose.position;
    rect.top_left = abs_points[0];
    rect.top_right = abs_points[1];
    rect.bottom_right = abs_points[2];
    rect.bottom_left = abs_points[3];
    rects.push_back(rect);
  }
  return rects;
}

/*
 * calculate distance in x-y 2D space
 */
std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vector<double> & y)
{
  if (x.size() != y.size()) {
    std::cerr << "x y vector size should be the same." << std::endl;
  }

  std::vector<double> dist_v;
  dist_v.push_back(0.0);
  for (unsigned int i = 0; i < x.size() - 1; ++i) {
    const double dx = x.at(i + 1) - x.at(i);
    const double dy = y.at(i + 1) - y.at(i);
    dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy));
  }

  return dist_v;
}

bool hasValidNearestPointFromEgo(
  const geometry_msgs::Pose & ego_pose, const Trajectories & trajs,
  const TrajectoryParam & traj_param)
{
  const auto traj = concatTraj(trajs);
  const auto interpolated_points =
    util::getInterpolatedPoints(traj, traj_param.delta_arc_length_for_trajectory);
  const int default_idx = -1;
  const int nearest_idx_with_thres = util::getNearestIdx(
    interpolated_points, ego_pose, default_idx, traj_param.delta_yaw_threshold_for_closest_point,
    traj_param.delta_dist_threshold_for_closest_point);
  if (nearest_idx_with_thres == default_idx) {
    return false;
  }
  return true;
}

std::vector<autoware_planning_msgs::TrajectoryPoint> concatTraj(const Trajectories & trajs)
{
  std::vector<autoware_planning_msgs::TrajectoryPoint> trajectory;
  trajectory.insert(
    trajectory.end(), trajs.model_predictive_trajectory.begin(),
    trajs.model_predictive_trajectory.end());
  trajectory.insert(
    trajectory.end(), trajs.extended_trajectory.begin(), trajs.extended_trajectory.end());
  return trajectory;
}

const int getZeroVelocityIdx(
  const bool is_showing_debug_info, const std::vector<geometry_msgs::Point> & fine_points,
  const std::vector<autoware_planning_msgs::PathPoint> & path_points,
  const std::unique_ptr<Trajectories> & opt_trajs, const TrajectoryParam & traj_param)
{
  const int default_idx = fine_points.size() - 1;
  const int zero_velocity_idx_from_path =
    getZeroVelocityIdxFromPoints(path_points, fine_points, default_idx, traj_param);

  int zero_velocity_idx_from_opt_points = zero_velocity_idx_from_path;
  if (opt_trajs) {
    zero_velocity_idx_from_opt_points = getZeroVelocityIdxFromPoints(
      util::concatTraj(*opt_trajs), fine_points, default_idx, traj_param);
  }
  ROS_INFO_COND(
    is_showing_debug_info, "0 m/s idx from path: %d from opt: %d total size %zu",
    zero_velocity_idx_from_path, zero_velocity_idx_from_opt_points, fine_points.size());
  // std::cout << "zero velo from path " << zero_velocity_idx_from_path << " from opt "
  //           << zero_velocity_idx_from_opt_points << " total size  " << fine_points.size()
  //           << std::endl;
  const int zero_velocity_idx =
    std::min(zero_velocity_idx_from_path, zero_velocity_idx_from_opt_points);

  if (is_showing_debug_info) {
    int zero_velocity_path_idx = path_points.size() - 1;
    for (int i = 0; i < path_points.size(); i++) {
      if (path_points[i].twist.linear.x < 1e-6) {
        zero_velocity_path_idx = i;
        break;
      }
    }
    const float debug_dist = util::calculate2DDistance(
      path_points[zero_velocity_path_idx].pose.position, fine_points[zero_velocity_idx]);
    ROS_INFO("Dist from path 0 velocity point: = %f [m]", debug_dist);
  }
  return zero_velocity_idx;
}

template <typename T>
int getZeroVelocityIdxFromPoints(
  const T & points, const std::vector<geometry_msgs::Point> & fine_points, const int default_idx,
  const TrajectoryParam & traj_param)
{
  int zero_velocity_points_idx = points.size() - 1;
  for (int i = 0; i < points.size(); i++) {
    if (points[i].twist.linear.x < 1e-6) {
      zero_velocity_points_idx = i;
      break;
    }
  }
  const int default_zero_velocity_fine_points_idx = fine_points.size() - 1;
  const int zero_velocity_fine_points_idx = util::getNearestIdx(
    fine_points, points[zero_velocity_points_idx].pose, default_zero_velocity_fine_points_idx,
    traj_param.delta_yaw_threshold_for_closest_point,
    traj_param.delta_dist_threshold_for_closest_point);
  return zero_velocity_fine_points_idx;
}
template int getZeroVelocityIdxFromPoints<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> &, const std::vector<geometry_msgs::Point> &,
  const int, const TrajectoryParam &);
template int getZeroVelocityIdxFromPoints<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &,
  const std::vector<geometry_msgs::Point> &, const int, const TrajectoryParam &);

template <typename T>
double getArcLength(const T & points, const int initial_idx)
{
  double accum_arc_length = 0;
  for (size_t i = initial_idx; i < points.size(); i++) {
    if (i > 0) {
      const double dist =
        util::calculate2DDistance(points[i].pose.position, points[i - 1].pose.position);
      accum_arc_length += dist;
    }
  }
  return accum_arc_length;
}
template double getArcLength<std::vector<autoware_planning_msgs::PathPoint>>(
  const std::vector<autoware_planning_msgs::PathPoint> &, const int);
template double getArcLength<std::vector<autoware_planning_msgs::TrajectoryPoint>>(
  const std::vector<autoware_planning_msgs::TrajectoryPoint> &, const int);

double getArcLength(const std::vector<geometry_msgs::Pose> & points, const int initial_idx)
{
  double accum_arc_length = 0;
  for (size_t i = initial_idx; i < points.size(); i++) {
    if (i > 0) {
      const double dist = util::calculate2DDistance(points[i].position, points[i - 1].position);
      accum_arc_length += dist;
    }
  }
  return accum_arc_length;
}

void logOSQPSolutionStatus(const int solution_status)
{
  /******************
  * Solver Status  *
  ******************/
  int OSQP_DUAL_INFEASIBLE_INACCURATE = 4;
  int OSQP_PRIMAL_INFEASIBLE_INACCURATE = 3;
  int OSQP_SOLVED_INACCURATE = 2;
  int OSQP_SOLVED = 1;
  int OSQP_MAX_ITER_REACHED = -2;
  int OSQP_PRIMAL_INFEASIBLE = -3; /* primal infeasible  */
  int OSQP_DUAL_INFEASIBLE = -4;   /* dual infeasible */
  int OSQP_SIGINT = -5;            /* interrupted by user */

  if (solution_status == OSQP_SOLVED) {
    // ROS_INFO("[Avoidance] OSQP solution status: OSQP_SOLVED");
  } else if (solution_status == OSQP_DUAL_INFEASIBLE_INACCURATE) {
    ROS_WARN("[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE_INACCURATE");
  } else if (solution_status == OSQP_PRIMAL_INFEASIBLE_INACCURATE) {
    ROS_WARN("[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE_INACCURATE");
  } else if (solution_status == OSQP_SOLVED_INACCURATE) {
    ROS_WARN("[Avoidance] OSQP solution status: OSQP_SOLVED_INACCURATE");
  } else if (solution_status == OSQP_MAX_ITER_REACHED) {
    ROS_WARN("[Avoidance] OSQP solution status: OSQP_ITER_REACHED");
  } else if (solution_status == OSQP_PRIMAL_INFEASIBLE) {
    ROS_WARN("[Avoidance] OSQP solution status: OSQP_PRIMAL_INFEASIBLE");
  } else if (solution_status == OSQP_DUAL_INFEASIBLE) {
    ROS_WARN("[Avoidance] OSQP solution status: OSQP_DUAL_INFEASIBLE");
  } else if (solution_status == OSQP_SIGINT) {
    ROS_WARN("[Avoidance] OSQP solution status: OSQP_SIGINT");
    ROS_WARN("[Avoidance] Interrupted by user, process will be finished.");
    std::exit(0);
  } else {
    ROS_WARN("[Avoidance] OSQP solution status: Not defined %d", solution_status);
  }
}

}  // namespace util


#include "Field.h"
#include "Encoding.h"
#include "Poly.h"
#include <iostream>
#include <cassert>
#include <time.h>
#include "timer.h"
#include "FileArchiver.h"
#include "Archive.h"

#include <boost/program_options.hpp>
using namespace std;
using namespace boost::program_options;

void randomFieldAddSubTest(Field* field, int num_tests) {
	FieldElt* f1 = field->newElt();
	FieldElt* f2 = field->newElt();
	FieldElt* f3 = field->newElt();
	FieldElt* result = field->newElt();

	for (int trial = 0; trial < num_tests; trial++) {
		field->assignRandomElt(f1);
		field->assignRandomElt(f2);
		field->assignRandomElt(f3);

		// Test reversability: f1 - f2 - f3 + f2 + f3 == f1
		field->sub(f1, f2, result);
		field->sub(result, f3, result);
		field->add(result, f2, result);
		field->add(result, f3, result);

		assert(field->equal(f1, result));
	}

	field->delElt(f1);
	field->delElt(f2);
	field->delElt(f3);
	field->delElt(result);
}

void randomFieldAssocTest(Field* field, int num_tests) {
	FieldElt* f1 = field->newElt();
	FieldElt* f2 = field->newElt();
	FieldElt* f3 = field->newElt();
	FieldElt* tmp = field->newElt();
	FieldElt* result1 = field->newElt();
	FieldElt* result2 = field->newElt();

	for (int trial = 0; trial < num_tests; trial++) {
		field->assignRandomElt(f1);
		field->assignRandomElt(f2);
		field->assignRandomElt(f3);

		// Test associativity of addition: f1 * (f2 + f3) == f1 * f2 + f1 * f3
		field->add(f2, f3, result1);
		field->mul(f1, result1, result1);

		field->mul(f1, f2, tmp);
		field->mul(f1, f3, result2);
		field->add(tmp, result2, result2);

		assert(field->equal(result1, result2));

		// Test associativity of subtraction: f1 * (f2 - f3) == f1 * f2 - f1 * f3
		field->sub(f2, f3, result1);
		field->mul(f1, result1, result1);

		field->mul(f1, f2, tmp);
		field->mul(f1, f3, result2);
		field->sub(tmp, result2, result2);

		assert(field->equal(result1, result2));
	}

	field->delElt(f1);
	field->delElt(f2);
	field->delElt(f3);
	field->delElt(tmp);
	field->delElt(result1);
	field->delElt(result2);
}

void randomFieldArrayTest(Field* field, int num_tests) {
	FieldEltArray* arr = field->newEltArray(num_tests, true);
	FieldElt* add_result1 = field->newElt();
	FieldElt* add_result2 = field->newElt();
	FieldElt* mul_result1 = field->newElt();
	FieldElt* mul_result2 = field->newElt();

	for (int i = 0; i < num_tests; i++) {
		// Randomize
		for (int j = 0; j < num_tests; j++) {
			field->assignRandomElt(arr->elt(j));
		}

		// Make sure we get the same result no matter how we access the array
		field->zero(add_result1);
		field->zero(add_result2);
		field->zero(mul_result1);
		field->zero(mul_result2);

		for (int j = 0; j < num_tests; j++) {
			field->add(add_result1, arr->elt(j),				 add_result1);
			field->add(add_result2, arr->elt(num_tests - j - 1), add_result2);

			field->mul(mul_result1, arr->elt(j),				 mul_result1);
			field->mul(mul_result2, arr->elt(num_tests - j - 1), mul_result2);
		}

		assert(field->equal(add_result1, add_result2));
		assert(field->equal(mul_result1, mul_result2));
	}

	delete arr;
	field->delElt(add_result1);
	field->delElt(add_result2);
	field->delElt(mul_result1);
	field->delElt(mul_result2);
}

void randomPolyMulTest(Field* field, int num_tests, int max_degree) {
	Poly p1(field), p2(field), resultFast(field), resultSlow(field);

	for (int trial = 0; trial < num_tests; trial++) {
		Poly::polyRand(field, p1, max_degree);
		Poly::polyRand(field, p2, max_degree);

		Poly::mul(p1, p2, resultFast);
		Poly::mulSlow(p1, p2, resultSlow);

		assert(resultFast.equals(resultSlow));
	}
}

void randomPolyAddSubTest(Field* field, int num_tests, int max_degree) {
	Poly p1(field), p2(field), p3(field), result(field);

	for (int trial = 0; trial < num_tests; trial++) {
		Poly::polyRand(field, p1, max_degree);
		Poly::polyRand(field, p2, max_degree);
		Poly::polyRand(field, p3, max_degree);

		// Test reversability: p1 - p2 - p3 + p2 + p3 == p1
		Poly::sub(p1, p2, result);
		Poly::sub(result, p3, result);
		Poly::add(result, p2, result);
		Poly::add(result, p3, result);

		assert(result.equals(p1));
	}
}

void randomPolyAddMulTest(Field* field, int num_tests, int max_degree) {
	Poly p1(field), p2(field), p3(field), result1(field), result2(field), temp(field);

	for (int trial = 0; trial < num_tests; trial++) {
		Poly::polyRand(field, p1, max_degree);
		Poly::polyRand(field, p2, max_degree);
		Poly::polyRand(field, p3, max_degree);

		// Test associativity: p1 * (p2 + p3) == p1 * p2 + p1 * p3
		Poly::add(p2, p3, result1);
		Poly::mul(p1, result1, result1);

		Poly::mul(p1, p2, temp);
		Poly::mul(p1, p3, result2);
		Poly::add(result2, temp, result2);

		assert(result1.equals(result2));
	}
}

void randomPolySubMulTest(Field* field, int num_tests, int max_degree) {
	Poly p1(field), p2(field), p3(field), result1(field), result2(field), temp(field);

	for (int trial = 0; trial < num_tests; trial++) {
		Poly::polyRand(field, p1, max_degree);
		Poly::polyRand(field, p2, max_degree);
		Poly::polyRand(field, p3, max_degree);

		// Test associativity: p1 * (p2 - p3) == p1 * p2 - p1 * p3
		Poly::sub(p2, p3, result1);
		Poly::mul(p1, result1, result1);

		Poly::mul(p1, p2, temp);
		Poly::mul(p1, p3, result2);
		Poly::sub(temp, result2, result2);

		assert(result1.equals(result2));
	}
}

void randomPolyModTest(Field* field, int num_tests, int max_degree) {
	Poly f(field), m(field), q(field), r(field), result(field);

	for (int trial = 0; trial < num_tests; trial++) {
		int modDegree = (int)((rand() / (double) RAND_MAX) * max_degree) + 1;	// +1, b/c we need mod to be non-constant
		assert(modDegree >= 1);

		Poly::polyRand(field, f, max_degree);
		Poly::polyRand(field, m, max_degree, modDegree);

		// Ensure m is monic
		field->one(m.coefficients->elt(m.getDegree()));

		Poly::mod(f, m, q, r);

		Poly::mul(q, m, result);
		Poly::add(result, r, result);

		assert(result.equals(f));
	}
}

// Setting geom to true fixes x values to powers of xr
void randomPolyEvalInterpTest(Field* field, int max_degree, int degree = -1, int numTests=100, bool geom=false) {

  printf("Interpolating polynomials with degree ");
  if (degree == -1) {
    printf("chosen at random\n");
  } else {
    printf("%d\n", degree);
  }

	Poly f(field);
	for (int trial = 0; trial < numTests; trial++) {
		// Pick a random function
		Poly::polyRand(field, f, max_degree, degree);

		// Pick some random points to evaluate at
		int numPts = f.getDegree() + 1;
		FieldEltArray* x = field->newEltArray(numPts, true);
		FieldEltArray* y = field->newEltArray(numPts, true);
		FieldElt* xr = field->newElt();
		field->set(xr, 3);

		for (int i = 0; i < numPts; i++) {
			if (!geom) field->assignRandomElt(x->elt(i));
			else if (i == 0) field->set(x->elt(i), 1);
			else field->mul(x->elt(i - 1), xr, x->elt(i));

			// Make sure this value is unique, since we'll be using it for interpolation
			bool unique;
			do {
				unique = true;
				for (int j = 0; j < i; j++) {
					if (field->equal(x->elt(i), x->elt(j))) {
						unique = false;
						field->assignRandomElt(x->elt(i));
						break;
					}
				}
			} while (!unique);
		}

		Poly::multiEval(f, x, numPts, y);
		Poly* fprime;
		if (!geom) fprime = Poly::interpolate(field, x, y, numPts);
		else fprime = Poly::interpolateGeometric(field, x->elt(0), xr, y, numPts);
		assert(f.equals(*fprime));

		delete x;
		delete y;
    delete xr;
		delete fprime;
	}
}

void randomEncAddSubTest(Encoding* encoding, elt_t t, int num_tests) {
	Field* field = encoding->getSrcField();
	FieldElt* f1 = field->newElt();
	FieldElt* f2 = field->newElt();
	FieldElt* f3 = field->newElt();

	EncodedElt* e1 = encoding->new_elt(t);
	EncodedElt* e2 = encoding->new_elt(t);
	EncodedElt* e3 = encoding->new_elt(t);
  EncodedElt* t1 = encoding->new_elt(t);
  EncodedElt* t2 = encoding->new_elt(t);
  EncodedElt* tmp1 = encoding->new_elt(t);
  EncodedElt* tmp2 = encoding->new_elt(t);
	EncodedElt* result = encoding->new_elt(t);

	for (int trial = 0; trial < num_tests; trial++) {
		field->assignRandomElt(f1);
		field->assignRandomElt(f2);
		field->assignRandomElt(f3);

		encoding->encode(t, f1, e1);
		encoding->encode(t, f2, e2);
		encoding->encode(t, f3, e3);

		// Basic equality checks
		assert(encoding->equal(t, e1, e1));
		assert(encoding->equal(t, e2, e2));
		assert(encoding->equal(t, e3, e3));

		assert(!encoding->equal(t, e1, e2));
		assert(!encoding->equal(t, e2, e3));

		// Check that ordering doesn't affect result
		encoding->add(t, e1, e2, result);
		encoding->add(t, result, e3, tmp1);

		encoding->add(t, e1, e3, result);
		encoding->add(t, result, e2, tmp2);
		assert(encoding->equal(t, tmp1, tmp2));

		encoding->sub(t, e1, e2, result);
		encoding->sub(t, result, e3, tmp1);

		encoding->sub(t, e1, e3, result);
		encoding->sub(t, result, e2, tmp2);
		assert(encoding->equal(t, tmp1, tmp2));

		// Test reversability: E(f1) - E(f2) - E(f3) + E(f2) + E(f3) == E(f1)
		encoding->sub(t, e1, e2, result);
		encoding->sub(t, result, e3, result);
		encoding->add(t, result, e2, result);
		encoding->add(t, result, e3, result);

		assert(encoding->equal(t, e1, result));
	}

	field->delElt(f1);
	field->delElt(f2);
	field->delElt(f3);

	encoding->del_elt(t, e1);
	encoding->del_elt(t, e2);
	encoding->del_elt(t, e3);
  encoding->del_elt(t, t1);
  encoding->del_elt(t, t2);
  encoding->del_elt(t, tmp1);
  encoding->del_elt(t, tmp2);
	encoding->del_elt(t, result);
}

void randomEncAssocTest(Encoding* encoding, elt_t t, int num_tests) {
	Field* field = encoding->getSrcField();
	FieldElt* f1 = field->newElt();
	FieldElt* f2 = field->newElt();
	FieldElt* f3 = field->newElt();
	FieldElt* tmp = field->newElt();
	FieldElt* result1 = field->newElt();

	EncodedElt* e1 = encoding->new_elt(t);
	EncodedElt* e2 = encoding->new_elt(t);
	EncodedElt* e3 = encoding->new_elt(t);
	EncodedElt* resultA = encoding->new_elt(t);
	EncodedElt* resultB = encoding->new_elt(t);

	for (int trial = 0; trial < num_tests; trial++) {
		field->assignRandomElt(f1);
		field->assignRandomElt(f2);
		field->assignRandomElt(f3);

		encoding->encode(t, f1, e1);
		encoding->encode(t, f2, e2);
		encoding->encode(t, f3, e3);

		// Test associativity of addition: f1 * (E(f2) + E(f3)) == f1 * E(f2) + f1 * E(f3) == E(f1*(f2+f3))
		encoding->add(t, e2, e3, resultA);
		encoding->mul(t, resultA, f1, resultA);

		encoding->mul(t, e2, f1, e2);
		encoding->mul(t, e3, f1, e3);
		encoding->add(t, e2, e3, resultB);

		assert(encoding->equal(t, resultA, resultB));

		field->add(f2, f3, result1);
		field->mul(f1, result1, result1);
		encoding->encode(t, result1, resultB);
		assert(encoding->equal(t, resultA, resultB));

		// Test associativity of subtraction: f1 * (E(f2) - E(f3)) == f1 * E(f2) - f1 * E(f3) == E(f1*(f2-f3))
		encoding->encode(t, f1, e1);
		encoding->encode(t, f2, e2);
		encoding->encode(t, f3, e3);

		encoding->sub(t, e2, e3, resultA);
		encoding->mul(t, resultA, f1, resultA);

		encoding->mul(t, e2, f1, e2);
		encoding->mul(t, e3, f1, e3);
		encoding->sub(t, e2, e3, resultB);

		assert(encoding->equal(t, resultA, resultB));

		field->sub(f2, f3, result1);
		field->mul(f1, result1, result1);
		encoding->encode(t, result1, resultB);

		assert(encoding->equal(t, resultA, resultB));
	}

	field->delElt(f1);
	field->delElt(f2);
	field->delElt(f3);
	field->delElt(tmp);
	field->delElt(result1);

	encoding->del_elt(t, e1);
	encoding->del_elt(t, e2);
	encoding->del_elt(t, e3);
	encoding->del_elt(t, resultA);
	encoding->del_elt(t, resultB);
}

void randomEncMulTest(Encoding* encoding, int num_tests) {
	Field* field = encoding->getSrcField();
	FieldElt* f1 = field->newElt();
	FieldElt* f2 = field->newElt();
	FieldElt* f3 = field->newElt();
	FieldElt* f4 = field->newElt();
	FieldElt* f5 = field->newElt();

	EncodedElt* L1 = encoding->new_elt(L);
	EncodedElt* R1 = encoding->new_elt(R);
	EncodedElt* L2 = encoding->new_elt(L);
	EncodedElt* R2 = encoding->new_elt(R);
	EncodedElt* Lsum = encoding->new_elt(L);
	EncodedElt* Rsum = encoding->new_elt(R);

	EncodedProduct* result1 = encoding->new_prod();
	EncodedProduct* result2 = encoding->new_prod();
	EncodedProduct* result3 = encoding->new_prod();

	for (int trial = 0; trial < num_tests; trial++) {
		field->assignRandomElt(f1);
		field->assignRandomElt(f2);
		field->assignRandomElt(f3);
		field->assignRandomElt(f4);

		// Very basic equality test for target group
		encoding->encode(L, f1, L1);  // L1 = g^f1
		encoding->encode(R, f2, R1);  // R1 = h^f2

		encoding->mul(L1, R1, result1);
		encoding->mul(L1, R1, result2);
		assert(encoding->equals(result1, result2));

		// Even simpler pairing check
		field->one(f5);
		encoding->encode(L, f1, L1);  // L1 = g^f1
		encoding->encode(R, f5, R1);  // R1 = h

		encoding->encode(L, f5, L2);  // L2 = g
		encoding->encode(R, f1, R2);  // R2 = h^f1

		encoding->mul(L1, R1, result1);	// result1 = e(g^f1, h)
		encoding->mul(L2, R2, result2); // result2 = e(g, h^f1)

		assert(encoding->equals(result1, result2));

		// Simple pairing check
		encoding->encode(L, f1, L1);  // L1 = g^f1
		encoding->encode(R, f2, R1);  // R1 = h^f2

		encoding->encode(L, f2, L2);  // L2 = g^f2
		encoding->encode(R, f1, R2);  // R2 = h^f1

		encoding->mul(L1, R1, result1);	// result1 = e(g^f1, h^f2)
		encoding->mul(L2, R2, result2); // result2 = e(g^f2, h^f1)
		assert(encoding->equals(result1, result2));

		// More complex pairing check
		encoding->encode(L, f1, L1);  // L1 = g^f1
		encoding->encode(R, f2, R1);  // R1 = h^f2

		encoding->mul(L, L1, f3, L2); // L2 = g^f1f3
		encoding->mul(R, R1, f4, R2); // R2 = h^f2f4

		encoding->mul(L2, R2, result1);	// e(g,h)^f1f2f3f4

		field->mul(f3, f4, f5);
		encoding->mul(L, L1, f5, L2);	  // L2 = g^f1f3f4
		encoding->mul(L2, R1, result2); // e(g,h)^f1f2f3f4

		encoding->mul(R, R1, f5, R2);	  // R2 = h^f2f3f4
		encoding->mul(L1, R2, result3); // e(g,h)^f1f2f3f4

		assert(encoding->equals(result1, result2));
		assert(encoding->equals(result2, result3));

		// One more test of operations in the target group
		field->assignRandomElt(f3);
		field->assignRandomElt(f4);

		encoding->encode(L, f1, L1);  // L1 = g^f1
		encoding->encode(R, f2, R1);  // R1 = h^f2

		encoding->encode(L, f3, L2);  // L2 = g^f3'
		encoding->encode(R, f4, R2);  // R2 = h^f4'

		encoding->add(L, L1, L2, Lsum);	// g^f1+f3'
		encoding->add(R, R1, R2, Rsum); // h^f2+f4'
		encoding->mul(Lsum, Rsum, result1);	// e(g,h)^{(f1+f3')(f2+f4')}

		encoding->mul(L1, R1, result2);	// e(g,h)^{f1f2}
		encoding->mul(L1, R2, result3);	// e(g,h)^{f1f4'}
		encoding->add(result2, result3, result3);
		encoding->mul(L2, R1, result2); // e(g,h)^{f3'f2}
		encoding->add(result2, result3, result3);
		encoding->mul(L2, R2, result2);	// e(g,h)^{f3'f4'}
		encoding->add(result2, result3, result3);		// e(g,h)^{(f1f2 + f1f4' + f3'f2 + f3'f4')

		assert(encoding->equals(result1, result3));
	}

	field->delElt(f1);
	field->delElt(f2);
	field->delElt(f3);
	field->delElt(f4);
	field->delElt(f5);

	encoding->del_elt(L, L1);
	encoding->del_elt(L, L2);
	encoding->del_elt(R, R1);
	encoding->del_elt(R, R2);
	encoding->del_elt(L, Lsum);
	encoding->del_elt(R, Rsum);
	delete result1;
	delete result2;
	delete result3;
}

void randomEncInnerProductTest(Encoding* encoding, elt_t t, int num_tests) {
	Field* field = encoding->getSrcField();

	FieldEltArray* exp = field->newEltArray(num_tests, true);
	FieldEltArray* randf = field->newEltArray(num_tests, true);
	EncodedEltArray* bases = encoding->new_elt_array(t, num_tests, true);

	EncodedElt* tmp = encoding->new_elt(t);
	EncodedElt* resultSlow = encoding->new_elt(t);
	EncodedElt* resultFast = encoding->new_elt(t);

	for (int test = 0; test < num_tests; test++) {
		// Choose all new random values
		for (int i = 0; i < num_tests; i++) {
			field->assignRandomElt(exp->elt(i));
			//field->one(exp->elt(i));
			field->assignRandomElt(randf->elt(i));
			encoding->encode(t, randf->elt(i), bases->elt(i));	// bases[i] = g^{r_i}
		}

		// Fast inner product
    int field_elt_max_bitsize = field->eltSize()*sizeof(digit_t) * 8;
    ip_handle_t h = encoding->prepareForInnerProduct(bases, num_tests, 5, field_elt_max_bitsize, 4, false);
		encoding->innerProduct(h, exp, num_tests, resultFast);
		encoding->doneWithInnerProduct(h);

		// Slow inner product
		encoding->zero(t, resultSlow);
		for (int i = 0; i < num_tests; i++) {
			encoding->mul(t, bases->elt(i), exp->elt(i), tmp);
			encoding->add(t, tmp, resultSlow, resultSlow);
		}

		assert(encoding->equal(t, resultSlow, resultFast));
	}

	delete exp;
	delete randf;
	delete bases;
	encoding->del_elt(t, tmp);
	encoding->del_elt(t, resultSlow);
	encoding->del_elt(t, resultFast);

}

void runFieldCorrectnessTest(Field* field, int num_tests) {
	randomFieldAddSubTest(field, num_tests);
	randomFieldAssocTest(field, num_tests);
	randomFieldArrayTest(field, num_tests);
}

void runPolyCorrectnessTest(Field* field, int num_tests, int max_degree) {
	randomPolyAddSubTest(field, num_tests, max_degree);
	randomPolyMulTest(field, num_tests, max_degree);
	randomPolyAddMulTest(field, num_tests, max_degree);
	randomPolySubMulTest(field, num_tests, max_degree);
	randomPolyModTest(field, num_tests, max_degree);
	randomPolyEvalInterpTest(field, max_degree, -1, num_tests);
	randomPolyEvalInterpTest(field, max_degree, -1, num_tests, true);
}

void runEncCorrectnessTestSided(Encoding* encoding, elt_t t, int num_tests) {
	randomEncAddSubTest(encoding, t, num_tests);
	randomEncAssocTest(encoding, t, num_tests);
	randomEncInnerProductTest(encoding, t, num_tests);
}

void runEncCorrectnessTest(Encoding* encoding, int num_tests) {
	runEncCorrectnessTestSided(encoding, L, num_tests);
	runEncCorrectnessTestSided(encoding, R, num_tests);
	randomEncMulTest(encoding, num_tests);
}

void runPrintTest(Encoding* encoding) {
	EncodedElt* eL = encoding->new_elt(L);
	EncodedElt* eR = encoding->new_elt(R);
	EncodedProduct* eP = encoding->new_prod();

	Field* field = encoding->getSrcField();
	FieldElt* f = field->newRandomElt();

	encoding->encode(L, f, eL);
	encoding->encode(R, f, eR);
	encoding->mul(eL, eR, eP);

	printf("\nField elt: ");
	field->print(f);
	printf("\nEncoded L elt: ");
	encoding->print(L, eL);
	printf("\nEncoded R elt: ");
	encoding->print(R, eR);
	printf("\nEncoded Product: ");
	encoding->print(eP);
	printf("\n");

	encoding->del_elt(L, eL);
	encoding->del_elt(R, eR);
	delete eP;
	field->delElt(f);
}

void Write(Archive* arc, Archiver* writer, Encoding* encoding, EncodedElt* eL, EncodedElt* eR, EncodedProduct* eP) {
  encoding->write(writer, L, eL, false);
  encoding->write(writer, R, eR, false);
  writer->write((uint8_t*)eP, encoding->size_of_prod());
}

void WriteTest(Encoding* encoding, int num_trials) {
  FileArchiver file_archiver("test_file.dat", FileArchiver::Write);
  Archive arc(&file_archiver, encoding);

  Field* field = encoding->getSrcField();
  FieldElt* f1 = field->newRandomElt();
  FieldElt* f2 = field->newRandomElt();
  EncodedElt* eL = encoding->new_elt(L);
  EncodedElt* eR = encoding->new_elt(R);
  EncodedProduct* eP = encoding->new_prod();

  // Write out some known values
  encoding->zero(L, eL);
  encoding->zero(R, eR);
  encoding->mul(eL, eR, eP);
  Write(&arc, &file_archiver, encoding, eL, eR, eP);
  encoding->one(L, eL);
  encoding->one(R, eR);
  encoding->mul(eL, eR, eP);
  Write(&arc, &file_archiver, encoding, eL, eR, eP);

  // Write out some random values
  for (int i = 0; i < num_trials; i++) {
    field->assignRandomElt(f1);
    field->assignRandomElt(f2);
    encoding->encode(L, f1, eL);
    encoding->encode(R, f2, eR);
    encoding->mul(eL, eR, eP);
    Write(&arc, &file_archiver, encoding, eL, eR, eP);
  }

  field->delElt(f1);
  field->delElt(f2);
  encoding->del_elt(L, eL);
  encoding->del_elt(R, eR);
  delete eP;
}

void Read(Archive* arc, Archiver* reader, Encoding* encoding, EncodedElt* eL, EncodedElt* eR, EncodedProduct* eP) {
  encoding->read(reader, L, eL);
  encoding->read(reader, R, eR);
  reader->read((uint8_t*)eP, encoding->size_of_prod());
}

void ReadTest(Encoding* encoding, int num_trials) {
  FileArchiver file_archiver("test_file.dat", FileArchiver::Read);
  Archive arc(&file_archiver, encoding);

  EncodedElt* eL = encoding->new_elt(L);
  EncodedElt* eR = encoding->new_elt(R);
  EncodedProduct* eP = encoding->new_prod();

  EncodedElt* tL = encoding->new_elt(L);
  EncodedElt* tR = encoding->new_elt(R);
  EncodedProduct* tP = encoding->new_prod();

  // Read in some known values
  Read(&arc, &file_archiver, encoding, eL, eR, eP);
  encoding->zero(L, tL);
  encoding->zero(R, tR);
  encoding->mul(tL, tR, tP);
  assert(encoding->equal(L, tL, eL));
  assert(encoding->equal(R, tR, eR));
  assert(encoding->equals(tP, eP));

  Read(&arc, &file_archiver, encoding, eL, eR, eP);
  encoding->one(L, tL);
  encoding->one(R, tR);
  encoding->mul(tL, tR, tP);
  assert(encoding->equal(L, tL, eL));
  assert(encoding->equal(R, tR, eR));
  assert(encoding->equals(tP, eP));

  // Read in some random values.  Can't check actual value,
  // but we can check that multiplication still works
  for (int i = 0; i < num_trials; i++) {
    Read(&arc, &file_archiver, encoding, eL, eR, eP);
    encoding->mul(eL, eR, tP);
    assert(encoding->equals(tP, eP));
  }

  encoding->del_elt(L, eL);
  encoding->del_elt(R, eR);
  delete eP;
  encoding->del_elt(L, tL);
  encoding->del_elt(R, tR);
  delete tP;
}

void runFileTest(Encoding* encoding, int num_trials) {
  WriteTest(encoding, num_trials);
  ReadTest(encoding, num_trials);
}

/*
void runCompressionTest(variables_map& config, Encoding* encoding, int num_trials) {
  if (!(config["encoding"].as<string>().compare("enigma-bn") == 0 || config["encoding"].as<string>().compare("arith-cp") == 0)) {
    printf("Compression not enabled for this encoding.  Skipping compression test\n");
    return;
  }

  Field* field = encoding->getSrcField();
  FieldElt* f1 = field->newElt();
  FieldElt* f2 = field->newElt();
  EncodedElt* eL = encoding->new_elt(L);
  EncodedElt* eR = encoding->new_elt(R);

  EncodedElt* eLc = encoding->new_elt(L);
  EncodedElt* eRc = encoding->new_elt(R);

  EncodedElt* eLd = encoding->new_elt(L);
  EncodedElt* eRd = encoding->new_elt(R);

  for (int i = 0; i < num_trials; i++) {
    field->assignRandomElt(f1);
    field->assignRandomElt(f2);

    encoding->encode(L, f1, eL);
    encoding->encode(R, f2, eR);

    encoding->copy(L, eL, eLc);
    encoding->copy(R, eR, eRc);

    encoding->compress(L, eLc);
    encoding->compress(R, eRc);

    if (config["encoding"].as<string>().compare("enigma-bn") == 0) {
      EncodedEltEnigmaBN* bnL = (EncodedEltEnigmaBN*)eLc;
      EncodedEltEnigmaBN* bnR = (EncodedEltEnigmaBN*)eRc;
      encoding->decompress(L, bnL->get_digits(), eLd);
      encoding->decompress(R, bnR->get_digits(), eRd);
    } else if (config["encoding"].as<string>().compare("arith-cp") == 0) {
      //EncodedEltArithCP* cpL = (EncodedEltArithCP*)eLc;
      //EncodedEltArithCP* cpR = (EncodedEltArithCP*)eRc;
      //encoding->decompress(L, cpL->me->X, eLd);
      //encoding->decompress(R, cpR->me->X, eRd);
      encoding->decompress(L, ((point_wp_t*) eLc)->X->limbs, eLd);
      encoding->decompress(R, ((point_wp2_t*)eRc)->X->limbs, eRd);
    }

    assert(encoding->equal(L, eL, eLd));
    assert(encoding->equal(R, eR, eRd));
  }

}
*/

void functionality_tests(variables_map& config) {
	Encoding* encoding = NewEncoding(config["encoding"].as<string>());
	if (encoding == NULL) { return; }
	Field* field = encoding->getSrcField();

	int max_degree = 200;
	int num_tests = config["trials"].as<int>();

	runFieldCorrectnessTest(field, num_tests);
	runPolyCorrectnessTest(field, num_tests, max_degree);
	runEncCorrectnessTest(encoding, num_tests);
	runPrintTest(encoding);
  //runFileTest(encoding, num_tests);

	encoding->prepareForManyEncodings(max_degree*10, 4, false);
	runEncCorrectnessTest(encoding, num_tests);  // Rerun with faster encodings
	encoding->doneWithManyEncodings();

	delete encoding;
}


//
// Created by Harold on 2021/4/12.
//

#ifndef M_MATH_M_CIRCSHIFT_EIGEN_HPP
#define M_MATH_M_CIRCSHIFT_EIGEN_HPP

// code from https://stackoverflow.com/questions/46077242/eigen-modifyable-custom-expression/46301503#46301503
// modified by Harold to meet Cpp11

#include <Eigen/Core>
#include <type_traits>

template <bool B> using bool_constant = std::integral_constant<bool, B>;

namespace helper
{
    namespace detail
    {
        template <typename T>
        constexpr std::true_type is_matrix(Eigen::MatrixBase<T>);
        std::false_type constexpr is_matrix(...);

        template <typename T>
        constexpr std::true_type is_array(Eigen::ArrayBase<T>);
        std::false_type constexpr is_array(...);
    }

    template <typename T>
    struct is_matrix : decltype(detail::is_matrix(std::declval<typename std::remove_cv<T>::type>()))
    {
    };

    template <typename T>
    struct is_array : decltype(detail::is_array(std::declval<typename std::remove_cv<T>::type>()))
    {
    };

    template <typename T>
    using is_matrix_or_array = bool_constant<is_array<T>::value || is_matrix<T>::value>;

/*
 * Index something if it's not an scalar
 */
    template <typename T, typename std::enable_if<is_matrix_or_array<T>::value, int>::type = 0>
    auto index_if_necessary(T&& thing, Eigen::Index idx) -> decltype(thing)
    {
        return thing(idx);
    }

/*
 * Overload for scalar.
 */
    template <typename T, typename std::enable_if<std::is_scalar<typename std::decay<T>::type>::value, int>::type = 0>
    auto index_if_necessary(T&& thing, Eigen::Index) -> decltype(thing)
    {
        return thing;
    }
}

namespace Eigen
{
    template <typename XprType, typename RowIndices, typename ColIndices>
    class CircShiftedView;

    namespace internal
    {
        template <typename XprType, typename RowIndices, typename ColIndices>
        struct traits<CircShiftedView<XprType, RowIndices, ColIndices>>
                : traits<XprType>
        {
            enum
            {
                RowsAtCompileTime = traits<XprType>::RowsAtCompileTime,
                ColsAtCompileTime = traits<XprType>::ColsAtCompileTime,
                MaxRowsAtCompileTime = (RowsAtCompileTime != Dynamic
                                        ? int(RowsAtCompileTime)
                                        : int(traits<XprType>::MaxRowsAtCompileTime)),
                MaxColsAtCompileTime = (ColsAtCompileTime != Dynamic
                                        ? int(ColsAtCompileTime)
                                        : int(traits<XprType>::MaxColsAtCompileTime)),
                XprTypeIsRowMajor = (int(traits<XprType>::Flags) & RowMajorBit) != 0,
                IsRowMajor = ((MaxRowsAtCompileTime == 1 && MaxColsAtCompileTime != 1) ? 1
                                                                                : (MaxColsAtCompileTime == 1 && MaxRowsAtCompileTime != 1) ? 0
                                                                                : XprTypeIsRowMajor),
                FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
                FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
                Flags = (traits<XprType>::Flags & HereditaryBits) | FlagsLvalueBit | FlagsRowMajorBit
            };
        };
    }

    template <typename XprType, typename RowShift, typename ColShift, typename StorageKind>
    class CircShiftedViewImpl;

    template <typename XprType, typename RowShift, typename ColShift>
    class CircShiftedView : public CircShiftedViewImpl<XprType, RowShift, ColShift,
            typename internal::traits<XprType>::StorageKind>
    {
    public:
        typedef typename CircShiftedViewImpl<XprType, RowShift, ColShift,
                typename internal::traits<XprType>::StorageKind>::Base Base;
        EIGEN_GENERIC_PUBLIC_INTERFACE(CircShiftedView)
        EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CircShiftedView)

        typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
        typedef typename internal::remove_all<XprType>::type NestedExpression;

        template <typename T0, typename T1>
        CircShiftedView(XprType& xpr, const T0& rowShift, const T1& colShift)
                : m_xpr(xpr), m_rowShift(rowShift), m_colShift(colShift)
        {
            for (auto c = 0; c < xpr.cols(); ++c)
                assert(std::abs(helper::index_if_necessary(m_rowShift, c)) < m_xpr.rows()); // row shift must be within +- rows()-1
            for (auto r = 0; r < xpr.rows(); ++r)
                assert(std::abs(helper::index_if_necessary(m_colShift, r)) < m_xpr.cols()); // col shift must be within +- cols()-1
        }

        /** \returns number of rows */
        Index rows() const { return m_xpr.rows(); }

        /** \returns number of columns */
        Index cols() const { return m_xpr.cols(); }

        /** \returns the nested expression */
        const typename internal::remove_all<XprType>::type&
        nestedExpression() const { return m_xpr; }

        /** \returns the nested expression */
        typename internal::remove_reference<XprType>::type&
        nestedExpression() { return m_xpr.const_cast_derived(); }

        EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
        Index getRowIdx(Index row, Index col) const
        {
            Index R = m_xpr.rows();
            assert(row >= 0 && row < R && col >= 0 && col < m_xpr.cols());
            Index r = row - helper::index_if_necessary(m_rowShift, col);
            if (r >= R)
                return r - R;
            if (r < 0)
                return r + R;
            return r;
        }

        EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
        Index getColIdx(Index row, Index col) const
        {
            Index C = m_xpr.cols();
            assert(row >= 0 && row < m_xpr.rows() && col >= 0 && col < C);
            Index c = col - helper::index_if_necessary(m_colShift, row);
            if (c >= C)
                return c - C;
            if (c < 0)
                return c + C;
            return c;
        }

    protected:
        MatrixTypeNested m_xpr;
        RowShift m_rowShift;
        ColShift m_colShift;
    };

// Generic API dispatcher
    template <typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
    class CircShiftedViewImpl
            : public internal::generic_xpr_base<CircShiftedView<XprType, RowIndices, ColIndices>>::type
    {
    public:
        typedef typename internal::generic_xpr_base<CircShiftedView<XprType, RowIndices, ColIndices>>::type Base;
    };

    namespace internal
    {
        template <typename ArgType, typename RowIndices, typename ColIndices>
        struct unary_evaluator<CircShiftedView<ArgType, RowIndices, ColIndices>, IndexBased>
                : evaluator_base<CircShiftedView<ArgType, RowIndices, ColIndices>>
        {
            typedef CircShiftedView<ArgType, RowIndices, ColIndices> XprType;

            enum
            {
                CoeffReadCost = (evaluator<ArgType>::CoeffReadCost
                                 + NumTraits<Index>::AddCost  /* for comparison */
                                 + NumTraits<Index>::AddCost) /* for addition */,
                Flags = (evaluator<ArgType>::Flags & HereditaryBits),
                Alignment = 0
            };

            EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
            {
                EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
            }

            typedef typename XprType::Scalar Scalar;
            typedef typename XprType::CoeffReturnType CoeffReturnType;

            EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
            CoeffReturnType coeff(Index row, Index col) const
            {
                return m_argImpl.coeff(m_xpr.getRowIdx(row, col), m_xpr.getColIdx(row, col));
            }

            EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
            CoeffReturnType coeff(Index idx) const
            {
                if (m_xpr.cols() == 1)
                    return m_argImpl.coeff(m_xpr.getRowIdx(idx, 1), 1);
                if (m_xpr.rows() == 1)
                    return m_argImpl.coeff(1, m_xpr.getColIdx(1, idx));
                assert(m_xpr.cols() == 1 || m_xpr.rows() == 1);
                // default no-assert case - assume col vector
                return m_argImpl.coeff(m_xpr.getRowIdx(idx, 1), 1);
            }

            EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
            Scalar& coeffRef(Index row, Index col)
            {
                assert(row >= 0 && row < m_xpr.rows() && col >= 0 && col < m_xpr.cols());

                return m_argImpl.coeffRef(m_xpr.getRowIdx(row, col), m_xpr.getColIdx(row, col));
            }

        protected:

            evaluator<ArgType> m_argImpl;
            const XprType& m_xpr;
        };
    } // end namespace internal
} // end namespace Eigen

#endif //M_MATH_M_CIRCSHIFT_EIGEN_HPP


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

#ifndef cal_ab_force_hpp
#define cal_ab_force_hpp

#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "maplocal.hpp"
#include "mapglobal.hpp"

using namespace Eigen;
extern double time_fem;
void cal_ab_force(MatrixXd &T_abc,VectorXd &F_total, VectorXi &ab_surf_index, VectorXd &v_n, int Ndofn);
#endif /* cal_ab_force_hpp */


#ifndef HELPER_HPP
#define HELPER_HPP

// Eigen related
#include <Eigen/Geometry>
// YAML related
#include <yaml-cpp/yaml.h>

// std
#include <fstream>
#include <iostream>

// ros
#include <geometry_msgs/TransformStamped.h>

namespace Utils {

/**
 * @brief from_quaternion_to_rot a function which creates rotation matrix from quaternion
 * @param q
 * @return
 */
template<typename T>
Eigen::Matrix<T, 3, 3> from_quaternion_to_rot(Eigen::Quaternion<T> &q) {
  return q.normalized().toRotationMatrix();
}

template<typename T>
Eigen::Matrix<T, 4, 4> from_rotation_translation_to_transformation(const Eigen::Matrix<T, 3, 3> &rotation, const Eigen::Matrix<T, 3, 1> &translation){
  Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();

  transformation.block<3, 3>(0, 0) = rotation;
  transformation.block<3, 1>(0, 3) = translation;
  return transformation;
}

}
#endif // HELPER_HPP


#pragma once

// C++ standard library
#include <unordered_map>
#include <vector>

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/armadillo.hpp"
// IWYU pragma: no_forward_declare mant::Hash
// IWYU pragma: no_forward_declare mant::IsEqual
namespace mant {
  class OptimisationProblem;
}

namespace mant {
  double fitnessDistanceCorrelation(
      const std::unordered_map<arma::vec, double, Hash, IsEqual>& samples);

  double lipschitzContinuity(
      const std::unordered_map<arma::vec, double, Hash, IsEqual>& samples);

  std::vector<arma::uvec> additiveSeparability(
      OptimisationProblem& optimisationProblem,
      const arma::uword numberOfEvaluations,
      const double minimalConfidence);
  std::vector<arma::uvec> additiveSeparability(
      OptimisationProblem& optimisationProblem,
      const arma::uword numberOfEvaluations);
}


#pragma once

#include "AK8963.hpp"
#include <Eigen/Dense>
#include <boost/endian/arithmetic.hpp>

namespace icarus
{
    namespace ak8963
    {
        enum class OperationMode : uint8_t
        {
            powerDown = 0b0000,
            singleMeasurement = 0b0001,
            continousMeasurement8Hz = 0b0010,
            continousMeasurement100Hz = 0b0110,
            externalTriggerMeasurement = 0b0100,
            selfTest = 0b1000,
            fuseRomAccess = 0b1111,
        };

        enum class OutputBitSetting : uint8_t
        {
            bits14 = 0,
            bits16 = 1,
        };

        struct Measurements
        {
            enum { address = 3 };

            Eigen::Matrix<boost::endian::little_int16_t, 3, 1> magneticField;
            uint8_t : 3;
            bool overflow : 1;
            OutputBitSetting outputBit : 1;
        };

        struct Control1
        {
            enum { address = 0xA };
            OperationMode mode : 4;
            OutputBitSetting outputBit : 1;
        };

        struct WhoAmI
        {
            enum { address = 0 };
            uint8_t data;
        };

        struct SensitivityAdjustment
        {
            enum { address = 16 };
            Eigen::Matrix<int8_t, 3, 1> adjustment;
        };
    }

    template<typename Delay, typename RegisterBank>
    AK8963<Delay, RegisterBank>::AK8963(RegisterBank * device) :
        mDevice(device)
    {}

    template<typename Delay, typename RegisterBank>
    void AK8963<Delay, RegisterBank>::initialize()
    {
        using namespace ak8963;

        mDevice->template write<Control1>([](auto & config) {
            config.mode = OperationMode::powerDown;
            config.outputBit = OutputBitSetting::bits14;
        });

        Delay::microseconds(100);

        mDevice->template write<Control1>([](auto & config) {
            config.mode = OperationMode::fuseRomAccess;
            config.outputBit = OutputBitSetting::bits14;
        });

        Delay::microseconds(100);

        mDevice->template read<SensitivityAdjustment>([this](auto & reg) {
            mScale = ((reg.adjustment.template cast<float>() / 256.0).array() + 1.0) * 0.6E-6;
        });

        mDevice->template write<Control1>([](auto & config) {
            config.mode = OperationMode::powerDown;
            config.outputBit = OutputBitSetting::bits14;
        });

        Delay::microseconds(100);

        mDevice->template write<Control1>([](auto & config) {
            config.mode = OperationMode::continousMeasurement100Hz;
            config.outputBit = OutputBitSetting::bits14;
        });

        Delay::microseconds(100);
    }

    template<typename Delay, typename RegisterBank>
    void AK8963<Delay, RegisterBank>::read()
    {
        using namespace ak8963;

        mDevice->template read<Measurements>([this](auto const& reg){
            mMagneticField = reg.magneticField.template cast<float>().cwiseProduct(mScale);
        });
    }
}


// Difference from running average, with multiprocessing.

// Import standard headers.
#include <vector>
#include <sstream>

// Import 3rd party headers.
#include <boost/date_time.hpp>
#include <opencv2/opencv.hpp>
#include <bites.hpp>

// Import application headers.
#include "sherlock.hpp"

void step1(
    cv::VideoCapture* cap,
    const int& duration,
    bites::ConcurrentQueue< cv::Mat* >* frames,
    bites::ConcurrentQueue< float >* alphas
    )
{
    // Keep track of previous iteration's timestamp.
    boost::posix_time::ptime tstamp_prev;

    // Run the loop for designated amount of time.
    auto now = boost::posix_time::microsec_clock::universal_time();
    auto dur = boost::posix_time::seconds(duration);
    auto end = now + dur;
    while (end > boost::posix_time::microsec_clock::universal_time())
    {
        // Take a snapshot.
        auto frame = new cv::Mat;
        *cap >> *frame;

        // Compute alpha value.
        auto alpha = sherlock::getAlpha(tstamp_prev, 1.0);

        // Push the frame and alpha value onto respective queues.
        frames->push(frame);
        alphas->push(alpha);
    }

    // Signal end-of-processing by pushing NULL value onto the queue.
    frames->push(NULL);
}

void step2(
    bites::ConcurrentQueue< cv::Mat* >* frames,
    bites::ConcurrentQueue< cv::Mat* >* diffs,
    bites::ConcurrentQueue< float >* alphas
    )
{
    // Maintain accumulation of differences.
    cv::Mat image_acc;

    // Pull from the queue while there are valid frames.
    cv::Mat* frame;
    frames->wait_and_pop(frame);
    while(frame){
        const auto RTYPE = CV_32FC3;

        // Initalize accumulation if so indicated.
        if (image_acc.empty()){
            image_acc = cv::Mat::zeros(frame->size(), frame->type());
            image_acc.convertTo(image_acc, RTYPE);
        }

        // Compute difference.
        cv::Mat converted;
        frame->convertTo(converted, RTYPE);
        auto image_diff = new cv::Mat;
        cv::absdiff(
            image_acc,
            converted,
            *image_diff
            );

        // Retrieve the alpha value.
        float alpha;
        alphas->wait_and_pop(alpha);

        // Accumulate the difference.
        cv::accumulateWeighted(
            converted,
            image_acc,
            alpha
            );

        // Convert the diff image, and push it onto the queue.
        image_diff->convertTo(*image_diff, frame->type());
        diffs->push(image_diff);

        // Original frame is no longer needed, so deallocate it.
        delete frame;

        // Retrieve the next frame from queue.
        frames->wait_and_pop(frame);
    }

    // Signal end-of-processing by pushing NULL value onto the queue.
    diffs->push(NULL);
}

int main(int argc, char** argv)
{
    // Parse command-line arguments.
    int DEVICE, WIDTH, HEIGHT;
    float DURATION;
    std::istringstream(std::string(argv[1])) >> DEVICE;
    std::istringstream(std::string(argv[2])) >> WIDTH;
    std::istringstream(std::string(argv[3])) >> HEIGHT;
    std::istringstream(std::string(argv[4])) >> DURATION;

    // Create the OpenCV video capture object.
    cv::VideoCapture cap(DEVICE);
    cap.set(3, WIDTH);
    cap.set(4, HEIGHT);

    // Create the output window.
    const char* title = "diff average 2";
    cv::namedWindow(title, CV_WINDOW_NORMAL);

    // Monitor framerates for the given seconds past.
    std::vector<float> periods = { 1, 5, 10 };
    bites::RateTicker framerate (periods);

    // Create the shared queues.
    bites::ConcurrentQueue <cv::Mat*> frames;
    bites::ConcurrentQueue <float> alphas;
    bites::ConcurrentQueue <cv::Mat*> diffs;

    // Start up the threads.
    std::thread thread1 (step1, &cap, DURATION, &frames, &alphas);
    std::thread thread2 (step2, &frames, &diffs, &alphas);

    // Pull from the queue while there are valid matrices.
    cv::Mat* diff;
    diffs.wait_and_pop(diff);
    while(diff){

        // Write the framerate on top of the image.
        auto fps = framerate.tick();
        std::ostringstream line;
        line << std::fixed << std::setprecision(2);
        line << fps[0] << ", " << fps[1] << ", " << fps[2];
        std::list<std::string> lines;
        lines.push_back(line.str());
        sherlock::writeOSD(*diff, lines, 0.04);

        // Display the snapshot.
        cv::imshow(title, *diff);

        // Allow HighGUI to process event.
        cv::waitKey(1);

        // Deallocate the current image, and retrieve the next.
        delete diff;
        diffs.wait_and_pop(diff);
    }

    // Join the two forked threads.
    thread1.join();
    thread2.join();
}