#include <string>
#include <iostream>
#include <fstream>
#include <gsl/gsl_errno.h>
#include <boost/program_options.hpp>
#include <Coss/VecD_io.h>
#include <Coss/py/CPyInstance.h>
#include "bpB/net/Net.h"
#include "bpB/net/Net_io.h"
#include "bpB/bp/BP_B.h"
#include "bpB/bp/BP_B_io.h"

// uncomment to disable assert()
// #define NDEBUG

int main(int const argc, char const* const argv[])
{
	// Turn off GSL error handling !! TODO: remove this later
	gsl_set_error_handler_off();

	std::string path;
	int iterations, seed;
	double tol, damp, p;
	bool regularize = false;

	boost::program_options::variables_map vm;
	boost::program_options::options_description desc("Program usage", 1024, 512);
	desc.add_options()
			("help,h", "produces this help message")
			("path", boost::program_options::value<std::string>(&path)->required(), "network path")
			("iterations,i", boost::program_options::value<int>(&iterations)->default_value(10000), "iterations")
			("tolerance,t", boost::program_options::value<double>(&tol)->default_value(1e-4), "used as stopping criteria")
			("damping,d", boost::program_options::value<double>(&damp)->default_value(0.3), "damping used in update_m")
			("msg,m", "if set, reads initial messages from path .msg")
			("sparse,s", "if set, reads stochiometric matrix in sparse representation")
			("prob,p", boost::program_options::value<double>(&p)->default_value(0.1), "0 < p <= 1 controls randomness of message passing schedule")
			("regularize,r", "if set, regularizes the bounds of m messages")
			("writemsg,w", "if set, writes messages at each iteration")
			("vols,v", "if set, outputs the volume computed at each iteration")
			("seed,e", boost::program_options::value<int>(&seed)->default_value(1), "seed for random generators");
	boost::program_options::positional_options_description p_desc;
	p_desc.add("path", 1);

	try
	{
		boost::program_options::store(boost::program_options::command_line_parser(argc, argv).
				  options(desc).positional(p_desc).run(), vm);
		if (vm.count("help"))
		{
			std::cout << desc << std::endl;
			return 1;
		}
		boost::program_options::notify(vm);
	}
	catch (std::exception& e)
	{
		std::cerr << "Error: " << e.what() << std::endl;
		return 1;
	}
	catch (...)
	{
		std::cerr << "Unknown error!" << std::endl;
		return 1;
	}

	if (iterations < 0)
	{
		std::cerr << "Error: iterations must be non-negative." << std::endl;
		return 1;
	}

	if (p <= 0 || p > 1)
	{
		std::cerr << "Error: p must be 0 <= p < 1." << std::endl;
		return 1;
	}

	Coss::bpB::Net net;

	if (vm.count("sparse"))
	{
		std::cout << "--sparse set. Reading sparse stoichiometric matrix representation." << std::endl;
		Coss::bpB::Net_io::read_sparse_net(path, net);
	}
	else
		Coss::bpB::Net_io::read_net(path, net);

	std::cout << "Stoichiometric matrix read with " << net.M() << " metabolites and " << net.N() << " reactions." << std::endl;

	Coss::bpB::BP_B bp(&net);
	bp.damp(damp).seed(seed).p(p);

	if (vm.count("regularize"))
		regularize = true;

	if (vm.count("msg"))
	{
		std::cout << "reading initialization messages" << std::endl;
		Coss::bpB::BP_B_io::read_M_msgs(path + ".msg", bp);
	}

	// initialize n messages from initial m messages
	bp.update_all_n();
	std::cout << "m and n messages initialized" << std::endl;

	if (vm.count("writemsg"))
		Coss::bpB::BP_B_io::export_msgs(bp, path + "_0.msg");

	if (vm.count("vols"))
	{
		auto entropies = bp.entropies();
		std::cout << "starting logvol=" << entropies.S << std::endl;
	}

	double max_change = 0;

	std::cout << "begin message passing. Using damp=" << damp << "." << std::endl;
	int itr;
	for (itr = 1; itr <= iterations; ++itr)
	{
		max_change = bp.update_all(regularize);
		if (vm.count("vols"))
		{
			auto entropies = bp.entropies();
			std::cout << "iteration " << itr << ", max_change=" << max_change
			<< ", logvol=" << entropies.S << std::endl;
		}
		else
			std::cout << "iteration " << itr << ", max_change=" << max_change
				 << std::endl;

		if (vm.count("writemsg"))
			Coss::bpB::BP_B_io::export_msgs(bp, path + "_" + std::to_string(itr) + ".msg");

		if (max_change <= tol)
			break;
	}

	std::cout << "exporting final messages to " << path + ".msg" << std::endl;
	Coss::bpB::BP_B_io::export_msgs(bp, path + ".msg");

	// compute volume
	std::cout << "computing entropies ..." << std::endl;
	auto entropies = bp.entropies();
	std::cout << "final S = " << entropies.S << std::endl;
	Coss::bpB::BP_B_io::export_Ss(bp, entropies, path);
	Coss::bpB::BP_B_io::export_Ks(bp, path);

	std::ofstream bpvol_file(path + ".bpvol");
	bpvol_file << entropies.S << std::endl;
	bpvol_file.close();

	std::ofstream logz_file(path + ".bplogz");
	logz_file << entropies.logZ << std::endl;
	logz_file.close();

	std::cout << "logvol written to " << path + ".bpvol" << std::endl;
	std::cout << "logZ written to " << path + ".bplogz" << std::endl;
}


#include <algorithm>
#include <cstring>
#include <limits>
#include <memory>
#include <queue>

#include <android/log.h>

#include <boost/foreach.hpp>
#include <boost/integer_traits.hpp>

#include <tbb/blocked_range2d.h>
#include <tbb/parallel_for.h>
#include <tbb/task_scheduler_init.h>

#include "body_parts_recognizer.h"
#include "cloud.h"
#include "stopwatch.h"

const Depth BACKGROUND_DEPTH = std::numeric_limits<Depth>::max();

template <typename Format> void
applyThreshold(ChannelRef<Format> & channel, Format threshold, Format bgValue)
{
  for (int i = 0; i < channel.size; ++i)
    if (channel.data[i] > threshold)
      channel.data[i] = bgValue;
}

template <typename Format> Format
selectThreshold(const ChannelRef<Format> & channel)
{
  // We use Otsu's method here.
  const int num_bins = boost::integer_traits<Format>::const_max + 1;
  int bins[num_bins] = { 0 };
  int count = 0;

  for (int i = 0; i < channel.size; ++i)
    if (channel.data[i] != BACKGROUND_DEPTH)
    {
      ++bins[channel.data[i]];
      ++count;
    }

  double scale = 1. / count;
  double mu = 0;

  for (int i = 0; i < num_bins; ++i) mu += i * bins[i] * scale;

  double mu1 = 0, mu2, p1 = 0, p2;
  double max_var = 0;
  Format thresh = 0;

  for (int i = 0; i < num_bins; ++i)
  {
    p1 += bins[i] * scale;
    p2 = 1 - p1;

    mu1 += i * bins[i] * scale;
    mu2 = mu - mu1;

    double var = p1 * p2 * (mu1 - mu2) * (mu1 - mu2);
    if (var > max_var)
    {
      max_var = var;
      thresh = i;
    }
  }

  return thresh;
}

template <typename Format> Format
replaceZeroes(ChannelRef<Format> & channel, Format replacement)
{
  std::replace(channel.data, channel.data + channel.size, Format(), replacement);
}

DECLARE_CLOUD_TAG(TagVisited, bool)

struct Component
{
  int y, x, surface;
};

struct ComponentSurfaceCompare
{
  bool operator() (const Component & c1, const Component & c2)
  { return c1.surface < c2.surface; }
};

template <typename Format> int
calcComponentSurface(const ChannelRef<Format> channel, ChannelRef<bool> visited, Format empty, int startY, int startX)
{
  std::queue<std::pair<int, int> > to_visit;
  to_visit.push(std::make_pair(startY, startX));
  int surface = 0;

  while (!to_visit.empty())
  {
    const std::pair<int, int> yx = to_visit.front();
    to_visit.pop();

    int y = yx.first, x = yx.second;

    if (x < 0 || x >= channel.width || y < 0 || y >= channel.height || visited.at(y, x) || channel.at(y, x) == empty) continue;

    visited.at(y, x) = true;
    ++surface;

    to_visit.push(std::make_pair(y, x + 1));
    to_visit.push(std::make_pair(y, x - 1));
    to_visit.push(std::make_pair(y + 1, x));
    to_visit.push(std::make_pair(y - 1, x));
  }

  return surface;
}

template <typename Format> void
eraseComponent(ChannelRef<Format> channel, Format empty, int y, int x)
{
  std::queue<std::pair<int, int> > to_visit;
  to_visit.push(std::make_pair(y, x));

  while (!to_visit.empty())
  {
    const std::pair<int, int> yx = to_visit.front();
    to_visit.pop();

    int y = yx.first, x = yx.second;

    if (x < 0 || x >= channel.width || y < 0 || y >= channel.height || channel.at(y, x) == empty) continue;

    channel.at(y, x) = empty;

    to_visit.push(std::make_pair(y, x + 1));
    to_visit.push(std::make_pair(y, x - 1));
    to_visit.push(std::make_pair(y + 1, x));
    to_visit.push(std::make_pair(y - 1, x));
  }
}

template <typename Format> Format
keepBiggestComponent(const ChannelRef<Format> channel, Format empty)
{
  Cloud temp;
  temp.resize(channel.width, channel.height);

  ChannelRef<bool> visited = temp.get<TagVisited>();
  std::fill_n(visited.data, visited.size, false);

  std::vector<Component> components;

  for (int i = 0; i < channel.height; ++i)
    for (int j = 0; j < channel.width; ++j)
      if (channel.at(i, j) != empty && !visited.at(i, j))
      {
        Component component = { i, j, calcComponentSurface(channel, visited, empty, i, j) };
        components.push_back(component);
      }

  const Component & biggest = *std::max_element(components.begin(), components.end(), ComponentSurfaceCompare());

  int with = 0;

  BOOST_FOREACH(const Component & c, components)
    if (c.surface < biggest.surface)
      eraseComponent(channel, empty, c.y, c.x);
}

struct DecisionTreeCPU
{
  tbb::task_scheduler_init tbb_init;

  struct Offsets
  {
    boost::int16_t du1, dv1, du2, dv2;
  };

  struct Node
  {
    Offsets offsets;
    boost::int16_t threshold;
  };

  boost::uint16_t depth;
  std::vector<Node> nodes;
  std::vector<Label> leaves;

  explicit DecisionTreeCPU(const char * data)
  {
    std::memcpy (&depth, data, sizeof depth);
    data += sizeof depth;

    nodes.resize ((1 << depth) - 1);
    std::memcpy (&nodes.front (), data, nodes.size () * sizeof (Node));
    data += nodes.size () * sizeof (Node);

    leaves.resize (1 << depth);
    std::memcpy (&leaves.front (), data, leaves.size () * sizeof (Label));
    data += leaves.size () * sizeof (Label);
  }

  Label
  walk(const ChannelRef<Depth> & depthChannel, int x, int y) const
  {
    unsigned nid = 0;
    Depth d0 = depthChannel.at(y, x);
    float scale = 1000.0f / d0;

    for(int node_depth = 0; node_depth < depth; ++node_depth)
    {
      const Node & node = nodes[nid];

      Depth d1 = depthChannel.atDef(y + node.offsets.dv1 * scale, x + node.offsets.du1 * scale, BACKGROUND_DEPTH);
      Depth d2 = depthChannel.atDef(y + node.offsets.dv2 * scale, x + node.offsets.du2 * scale, BACKGROUND_DEPTH);

      int feature = int (d1) - int (d2);

      if (feature > node.threshold)
        nid = nid * 2 + 2;
      else
        nid = nid * 2 + 1;
    }

    return leaves[nid - nodes.size()];
  }

  struct WalkHelper
  {
  private:
    const DecisionTreeCPU & tree;
    const ChannelRef<Depth> depth;
    std::vector<Label> & labels;

  public:
    WalkHelper(const DecisionTreeCPU & tree, Cloud & cloud, std::vector<Label> & labels)
      : tree(tree), depth(cloud.get<TagDepth>()), labels(labels)
    {
    }

    void
    operator () (const tbb::blocked_range2d<unsigned> & range) const
    {
      for (unsigned y = range.rows().begin(); y < range.rows().end(); ++y)
        for (unsigned x = range.cols().begin(); x < range.cols().end(); ++x)
          labels[x + y * depth.width] = depth.at(y, x) == BACKGROUND_DEPTH ?
                Labels::Background : tree.walk(depth, x, y);
    }
  };

  void
  eval(Cloud & cloud, std::vector<Label> & labels) const
  {
    tbb::parallel_for(
          tbb::blocked_range2d<unsigned>(0, cloud.getHeight(), 0, cloud.getWidth()),
          WalkHelper(*this, cloud, labels)
    );
  }
};

int maxElementNoTie(int num, unsigned * elements)
{
  int max_element = 0;
  unsigned max = elements[max_element];

  for (int i = 1; i < num; ++i)
  {
    unsigned val = elements[i];
    if (max < val) { max_element = i; max = val; }
    else if (max == val) { max_element = -1; }
  }

  return max_element;
}

struct FilterHelper
{
private:
  ChannelRef<Label> noisy, good;
  int radius;

public:
  FilterHelper(ChannelRef<Label> noisy, ChannelRef<Label> good, int radius)
    : noisy(noisy), good(good), radius(radius)
  { }

  void
  operator ()(const tbb::blocked_range2d<int> & range) const
  {
    int width = noisy.width, height = noisy.height;

    for (int i = range.rows().begin(); i < range.rows().end(); ++i)
      for (int j = range.cols().begin(); j < range.cols().end(); ++j)
      {
        int idx = i * width + j;

        if (i < radius || i >= height - radius || j < radius || j >= width - radius || noisy.data[idx] == Labels::Background)
        {
          good.data[idx] = Labels::Background;
          continue;
        }

        int bins[Labels::NUM_LABELS] = { 0 };
        Label mode = -1;
        Label mode_count = 0;

        for (int dy = -radius; dy <= radius; ++dy)
          for (int dx = -radius; dx <= radius; ++dx)
          {
            Label current = noisy.data[idx + dx + dy * width];
            ++bins[current];
            if (bins[current] > mode_count) {
              mode_count = bins[current];
              mode = current;
            }
          }

        good.data[idx] = mode;
      }
  }
};

void filterLabels(Cloud & noisy, Cloud & output, int radius)
{
  int width = noisy.getWidth(), height = noisy.getHeight();
  ChannelRef<Label> noisy_labels = noisy.get<TagBPLabel>();
  ChannelRef<Label> good_labels = output.get<TagBPLabel>();

  tbb::parallel_for(
        tbb::blocked_range2d<int>(0, noisy.getHeight(), 0, noisy.getWidth()),
        FilterHelper(noisy_labels, good_labels, radius)
  );
}

struct ConsensusHelper
{
private:
  const std::vector<std::vector<Label> > & multi_labels;
  ChannelRef<Label> labels;
  const ChannelRef<Depth> depths;

public:
  ConsensusHelper(
      const std::vector<std::vector<Label> > & multi_labels,
      Cloud & cloud
  )
    : multi_labels(multi_labels), labels(cloud.get<TagBPLabel>()), depths(cloud.get<TagDepth>())
  { }

  void
  operator ()(const tbb::blocked_range2d<unsigned> & range) const
  {
    for (unsigned y = range.rows().begin(); y < range.rows().end(); ++y)
      for (unsigned x = range.cols().begin(); x < range.cols().end(); ++x)
      {
        std::size_t i = &depths.at(y, x) - depths.data;

        bool background = true;
        for (std::size_t ti = 0; ti < multi_labels.size (); ++ti)
          if (multi_labels[ti][i] != Labels::Background) background = false;

        if (background)
        {
          labels.data[i] = Labels::Background;
          continue;
        }

        unsigned bins[Labels::NUM_LABELS] = { 0 };

        for (std::size_t ti = 0; ti < multi_labels.size (); ++ti)
          ++bins[multi_labels[ti][i]];

        int consensus = maxElementNoTie(Labels::NUM_LABELS, bins);

        if (consensus == -1)
        {
          std::fill (bins, bins + Labels::NUM_LABELS, 0);
          Depth d = depths.at(y, x);

          for (int off_x = -1; off_x <= 1; ++off_x)
            for (int off_y = -1; off_y <= 1; ++off_y)
            {
              Depth off_d = depths.atDef(y + off_y, x + off_x, BACKGROUND_DEPTH);

              if (std::abs (d - off_d) < 50)
                for (std::size_t ti = 0; ti < multi_labels.size (); ++ti)
                  ++bins[multi_labels[ti][i]];
            }

          labels.data[i] = std::max_element (bins, bins + Labels::NUM_LABELS) - bins;
        }
        else
        {
          labels.data[i] = consensus;
        }
      }
  }
};

void
findConsensus(const std::vector<std::vector<Label> > & multi_labels, Cloud & cloud)
{
  tbb::parallel_for(
        tbb::blocked_range2d<unsigned>(0, cloud.getHeight(), 0, cloud.getWidth()),
        ConsensusHelper(multi_labels, cloud)
  );
}

BodyPartsRecognizer::BodyPartsRecognizer(std::size_t num_trees, const char * trees[])
{
  this->trees.resize(num_trees);

  for (std::size_t i = 0; i < num_trees; ++i)
    this->trees[i].reset(new Tree(trees[i]));
}

void
BodyPartsRecognizer::recognize(Cloud & cloud) const
{
  ChannelRef<Depth> depth = cloud.get<TagDepth>();

  Stopwatch watch_threshold;

  replaceZeroes(depth, BACKGROUND_DEPTH);
  applyThreshold(depth, selectThreshold(depth), BACKGROUND_DEPTH);
  keepBiggestComponent(depth, BACKGROUND_DEPTH);

  __android_log_print(ANDROID_LOG_INFO, "BPR", "Thresholding: %d ms", watch_threshold.elapsedMs());

  std::vector<std::vector<Label> > multi_labels (trees.size ());

  for (std::size_t ti = 0; ti < trees.size (); ++ti)
  {
    Stopwatch watch_evaluation;

    multi_labels[ti].resize (cloud.getHeight() * cloud.getWidth());
    trees[ti]->eval (cloud, multi_labels[ti]);

    __android_log_print(ANDROID_LOG_INFO, "BPR", "Evaluating tree %d: %d ms", ti, watch_evaluation.elapsedMs());
  }

  Stopwatch watch_consensus;

  Cloud noisy;
  noisy.resize(cloud.getWidth(), cloud.getHeight());

  findConsensus(multi_labels, noisy);

  __android_log_print(ANDROID_LOG_INFO, "BPR", "Finding consensus: %d ms", watch_consensus.elapsedMs());

  Stopwatch watch_filtering;

  filterLabels(noisy, cloud, 2);

  __android_log_print(ANDROID_LOG_INFO, "BPR", "Filtering labels: %d ms", watch_consensus.elapsedMs());

}


#ifndef ALGORITHM_WDTOCS_HPP_XB3U83S3CR1
#define ALGORITHM_WDTOCS_HPP_XB3U83S3CR1
#include <stdlib.h>
#include <log4cxx/logger.h>
#include <log4cxx/basicconfigurator.h>
#include <log4cxx/helpers/exception.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/unordered_map.hpp>
#include <boost/multi_array.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include "algorithm_distance_map.hpp"

//namespace std {using namespace __gnu_cxx;}
using namespace std;

namespace prhlt {
    class Algorithm_WDTOCS: public Algorithm_Distance_Map{
        public:
            Algorithm_WDTOCS(cv::Mat &ex_image);
            void run(int ex_curvature_ratio, int ex_threshold,float ex_direct_constant, float ex_diagonal_constant);
            void run(int ex_curvature_ratio, cv::Mat frontier_points_mat,float ex_direct_constant, float ex_diagonal_constant);
        protected:
            float direct_distance_constant;
            float diagonal_distance_constant;
            virtual float calculate_neighbour_value(int r1,int c1, int r2, int c2);
            float calculate_grey_gradient(int r1,int c1, int r2, int c2);
            float calculate_straight_path_grey_gradient(int r1,int c1, int r2, int c2);
            float calculate_diagonal_path_grey_gradient(int r1,int c1, int r2, int c2);
	};
}

#endif /* end of include guard*/


#include <boost/pending/fibonacci_heap.hpp>


#include "core/simulations/PuckPush.h"

#include "core/Util.h"
#include <boost/math/tools/roots.hpp>
#include <functional>
#include <iostream>
#include <opencv2/imgproc.hpp>
#include <random>

namespace simulations {

const boost::geometry::model::ring<vector_t, true, false> BOARD_REGION_BOT_BOOST(
  PuckPush::BOARD_REGION_BOT.begin(), PuckPush::BOARD_REGION_BOT.end());

const boost::geometry::model::ring<vector_t, true, false> BOARD_REGION_PUCK_BOOST(
  PuckPush::BOARD_REGION_PUCK.begin(), PuckPush::BOARD_REGION_PUCK.end());

PuckPush::Action PuckPush::Action::Rand() {
  return {std::uniform_real_distribution<float>(0, 2 * PI)(Rng())};
}

list_t<list_t<PuckPush::Action>> PuckPush::Action::CreateHandcrafted(size_t length) {
  list_t<list_t<Action>> macro_actions;
  for (size_t i = 0; i < 8; i++) {
    macro_actions.emplace_back();
    for (size_t j = 0; j < length; j++) {
      macro_actions.back().push_back({static_cast<float>(i) * 2 * PI / 8});
    }
  }
  return macro_actions;
}

list_t<list_t<PuckPush::Action>> PuckPush::Action::Deserialize(const list_t<float>& params, size_t macro_length) {
  return StandardMacroActionDeserialization<PuckPush::Action>(params, macro_length);
}

uint64_t PuckPush::Observation::Discretize() const {
  list_t<int> data {
    static_cast<int>(floorf(bot_position.x / 1.0f)),
    static_cast<int>(floorf(bot_position.y / 1.0f)),
    static_cast<int>(floorf(puck_position.x / 1.0f)),
    static_cast<int>(floorf(puck_position.y / 1.0f))
  };
  return boost::hash_value(data);
}

PuckPush::PuckPush() : step(0), _is_terminal(false), _is_failure(false) {

}

PuckPush PuckPush::CreateRandom() {

  if constexpr (RANDOMIZE_GOAL) {
    GOAL.x = std::uniform_real_distribution<float>(GOAL_REGION[0].x, GOAL_REGION[2].x)(Rng());
    GOAL.y = std::uniform_real_distribution<float>(GOAL_REGION[0].y, GOAL_REGION[2].y)(Rng());
  } else {
    GOAL = {1058, 428};
  }

  return SampleBeliefPrior();
}

/* ====== Belief related functions ====== */
PuckPush PuckPush::SampleBeliefPrior() {
  PuckPush sim;
  sim.bot_position = BOT_START_POSITION;
  sim.bot_position.x += std::normal_distribution<float>(0.0, INITIAL_POSITION_NOISE)(Rng());
  sim.bot_position.y += std::normal_distribution<float>(0.0, INITIAL_POSITION_NOISE)(Rng());
  sim.puck_position = PUCK_START_POSITION;
  sim.puck_position.x += std::normal_distribution<float>(0.0, INITIAL_POSITION_NOISE)(Rng());
  sim.puck_position.y += std::normal_distribution<float>(0.0, INITIAL_POSITION_NOISE)(Rng());
  return sim;
}

float PuckPush::Error(const PuckPush& other) const {
  float error = 0;
  error += (bot_position - other.bot_position).norm();
  error += (puck_position - other.puck_position).norm();
  return error / 2;
}

/* ====== Bounds related functions ====== */
float PuckPush::BestReward() const {
  if (_is_terminal) { return 0; } // Return value of 0 needed for DESPOT.
  float distance = (GOAL - puck_position).norm() - (GOAL_RADIUS - PUCK_RADIUS);
  float max_distance_per_step = BOT_SPEED * DELTA;
  size_t steps = static_cast<size_t>(round(ceilf(distance / max_distance_per_step)));
  if (steps <= 1) {
    return GOAL_REWARD;
  } else {
    return (1 - powf(GAMMA, static_cast<float>(steps) - 1)) / (1 - static_cast<float>(steps)) * STEP_REWARD +
      powf(GAMMA, static_cast<float>(steps) - 1) * GOAL_REWARD;
  }
}

/* ====== Stepping functions ====== */
template <bool compute_log_prob>
std::tuple<PuckPush, float, PuckPush::Observation, float> PuckPush::Step(
      const PuckPush::Action& action, const PuckPush::Observation* observation) const {
  if (_is_terminal) { throw std::logic_error("Cannot step terminal simulation."); }

  PuckPush next_sim = *this;
  float reward;

  /* ====== Step 1: Update state. ====== */

  // Sample constants.
  vector_t bot_dir = vector_t(1, 0).rotated(action.orientation +
      std::normal_distribution<float>(0.0f, BOT_MOVE_ORIENTATION_NOISE)(RngDet()));
  float bot_speed;
  do {
    bot_speed = std::normal_distribution<float>(BOT_SPEED, BOT_SPEED_NOISE)(RngDet());
  } while (bot_speed <= 0);
  float puck_bot_roll_constant;
  do {
    puck_bot_roll_constant = std::normal_distribution<float>(PUCK_BOT_ROLL_CONSTANT, PUCK_BOT_ROLL_CONSTANT_NOISE)(RngDet());
  } while (puck_bot_roll_constant <= 0);
  float puck_bot_roll_displacement;
  do {
    puck_bot_roll_displacement = std::normal_distribution<float>(PUCK_BOT_ROLL_DISPLACEMENT, PUCK_BOT_ROLL_DISPLACEMENT_NOISE)(RngDet());
  } while (puck_bot_roll_displacement < 0);

  // Step puck.
  vector_t bot_segment_start = next_sim.bot_position;
  vector_t bot_segment_end = next_sim.bot_position + bot_dir * bot_speed * DELTA;
  float bot_segment_length = (bot_segment_end - bot_segment_start).norm();
  std::optional<float> intersection_ratio = FindFirstRootQuadratic(
      (bot_segment_end - bot_segment_start).squaredNorm(),
      2 * (bot_segment_start - next_sim.puck_position).dot(bot_segment_end - bot_segment_start),
      (bot_segment_start - next_sim.puck_position).squaredNorm() - (PUCK_RADIUS + BOT_RADIUS) * (PUCK_RADIUS + BOT_RADIUS),
      0.0f, 1.0f);
  if (intersection_ratio) {
    vector_t intersecting_bot_pos = bot_segment_start + *intersection_ratio * (bot_segment_end - bot_segment_start);
    float initial_angle = AngleTo(bot_dir, next_sim.puck_position - intersecting_bot_pos);
    float roll_distance = std::min(
        (1 - *intersection_ratio) * bot_segment_length,
        logf(PI / (2 * std::abs(initial_angle))) / puck_bot_roll_constant);
    float final_angle = initial_angle * expf(puck_bot_roll_constant * roll_distance);
    next_sim.puck_position = bot_segment_start +
      std::min(1.0f, *intersection_ratio + roll_distance / bot_segment_length) * (bot_segment_end - bot_segment_start) +
      (1 + 0.001f + puck_bot_roll_displacement) * (PUCK_RADIUS + BOT_RADIUS) * bot_dir.rotated(final_angle);
  }
  // Step bot.
  next_sim.bot_position = bot_segment_end;

  next_sim.step++;

  // Check terminal and calculate rewards.
  if (!boost::geometry::within(next_sim.bot_position, BOARD_REGION_BOT_BOOST)) {
    next_sim._is_terminal = true;
    next_sim._is_failure = true;
    reward = COLLISION_REWARD;
  }
  if (!next_sim._is_terminal) {
    if (!boost::geometry::within(next_sim.puck_position, BOARD_REGION_PUCK_BOOST)) {
      next_sim._is_terminal = true;
      next_sim._is_failure = true;
      reward = COLLISION_REWARD;
    }
  }
  if (!next_sim._is_terminal) {
    if ((next_sim.puck_position - GOAL).norm() <= GOAL_RADIUS - PUCK_RADIUS) {
      next_sim._is_terminal = true;
      reward = GOAL_REWARD;
    }
  }
  if (!next_sim._is_terminal) {
    if (next_sim.step == MAX_STEPS) {
      reward = COLLISION_REWARD;
      next_sim._is_terminal = true;
      next_sim._is_failure = true;
    }
  }
  if (!next_sim._is_terminal) {
    reward = STEP_REWARD;
  }

  if (reward != STEP_REWARD && reward != GOAL_REWARD && reward != COLLISION_REWARD) {
    throw std::logic_error("INVALID REWARD!");
  }

  /* ====== Step 2: Generate observation. ====== */

  Observation new_observation;
  if (observation) {
    new_observation = *observation;
  }
  float log_prob = 0;

  bool in_noisy_region = false;
  for (const auto& noisy_region : NOISY_REGIONS) {
    if (next_sim.puck_position.x >= noisy_region[0].x && next_sim.puck_position.x <= noisy_region[1].x) {
      if (next_sim.puck_position.y >= noisy_region[0].y && next_sim.puck_position.y <= noisy_region[1].y) {
        in_noisy_region = true;
        break;
      }
    }
  }

  if (!observation) {
    new_observation.bot_position = next_sim.bot_position;
    new_observation.bot_position.x += std::normal_distribution<float>(0.0, BOT_OBSERVATION_NOISE)(RngDet());
    new_observation.bot_position.y += std::normal_distribution<float>(0.0, BOT_OBSERVATION_NOISE)(RngDet());
    if (!in_noisy_region) {
      if (std::uniform_real_distribution<float>(0.0f, 1.0f)(RngDet()) < PUCK_FLICKER_PROBABILITY) {
        new_observation.puck_position.x = std::numeric_limits<float>::quiet_NaN();
        new_observation.puck_position.y = std::numeric_limits<float>::quiet_NaN();
      } else {
        new_observation.puck_position = next_sim.puck_position;
        new_observation.puck_position.x += std::normal_distribution<float>(0.0, PUCK_OBSERVATION_NOISE)(RngDet());
        new_observation.puck_position.y += std::normal_distribution<float>(0.0, PUCK_OBSERVATION_NOISE)(RngDet());
      }
    } else {
      new_observation.puck_position.x = std::numeric_limits<float>::quiet_NaN();
      new_observation.puck_position.y = std::numeric_limits<float>::quiet_NaN();
    }
  }
  if (compute_log_prob) {
    log_prob += NormalLogProb(next_sim.bot_position.x, BOT_OBSERVATION_NOISE, new_observation.bot_position.x);
    log_prob += NormalLogProb(next_sim.bot_position.y, BOT_OBSERVATION_NOISE, new_observation.bot_position.y);
    if (!in_noisy_region) {
      if (std::isnan(new_observation.puck_position.x) && std::isnan(new_observation.puck_position.y)) {
        // Flickering outside noisy regions.
        log_prob += logf(PUCK_FLICKER_PROBABILITY);
      } else {
        log_prob += NormalLogProb(next_sim.puck_position.x, PUCK_OBSERVATION_NOISE, new_observation.puck_position.x);
        log_prob += NormalLogProb(next_sim.puck_position.y, PUCK_OBSERVATION_NOISE, new_observation.puck_position.y);
      }
    } else {
      if (std::isnan(new_observation.puck_position.x) && std::isnan(new_observation.puck_position.y)) {
        // Missing observation inside noisy regions.
        log_prob += 0;
      } else {
        log_prob += -std::numeric_limits<float>::infinity();
      }
    }
  }

  return std::make_tuple(next_sim, reward, observation ? Observation() : new_observation, log_prob);
}
template std::tuple<PuckPush, float, PuckPush::Observation, float> PuckPush::Step<true>(
    const PuckPush::Action& action, const PuckPush::Observation* observation) const;
template std::tuple<PuckPush, float, PuckPush::Observation, float> PuckPush::Step<false>(
    const PuckPush::Action& action, const PuckPush::Observation* observation) const;

/* ====== Serialization functions ====== */

void PuckPush::Encode(list_t<float>& data) const {
  data.emplace_back(static_cast<float>(step));
  bot_position.Encode(data);
  puck_position.Encode(data);
}

void PuckPush::EncodeContext(list_t<float>& data) {
  if constexpr (RANDOMIZE_GOAL) {
    GOAL.Encode(data);
  }
}

cv::Mat PuckPush::Render(const list_t<PuckPush>& belief_sims,
    const list_t<list_t<Action>>& macro_actions, const vector_t& macro_action_start) const {

  constexpr float SCENARIO_MIN_HORIZONTAL = -30.0f;
  constexpr float SCENARIO_MAX_HORIZONTAL = 1310.0f;
  constexpr float SCENARIO_MIN_VERTICAL = -30.0f;
  constexpr float SCENARIO_MAX_VERTICAL = 617.0f;
  constexpr float RESOLUTION = 1.0f;
  auto to_frame = [&](const vector_t& vector) {
    return cv::Point{
      static_cast<int>((vector.x - SCENARIO_MIN_HORIZONTAL) / RESOLUTION),
      static_cast<int>((vector.y - SCENARIO_MIN_VERTICAL) / RESOLUTION)
    };
  };
  auto to_frame_dist = [&](float d) {
    return static_cast<int>(d / RESOLUTION);
  };

  cv::Mat frame(
      static_cast<int>((SCENARIO_MAX_VERTICAL - SCENARIO_MIN_VERTICAL) / RESOLUTION),
      static_cast<int>((SCENARIO_MAX_HORIZONTAL - SCENARIO_MIN_HORIZONTAL) / RESOLUTION),
      CV_8UC3,
      cv::Scalar(255, 255, 255));

  // Draw noisy
  for (size_t i = 0; i < NOISY_REGIONS.size(); i++) {
    cv::rectangle(frame,
        to_frame(NOISY_REGIONS[i][0] + vector_t{PUCK_RADIUS, PUCK_RADIUS}),
        to_frame(NOISY_REGIONS[i][1] - vector_t{PUCK_RADIUS, PUCK_RADIUS}),
        cv::Scalar(25, 211, 255),
        -1, cv::LINE_AA);
  }

  // Draw regions region.
  cv::rectangle(frame, to_frame(BOARD_REGION[0]), to_frame(BOARD_REGION[2]),
      cv::Scalar(0, 0, 0), 5, cv::LINE_AA);
  cv::rectangle(frame,
      to_frame(BOT_START_REGION[0] - BOT_RADIUS * vector_t{1, 1}),
      to_frame(BOT_START_REGION[2] + BOT_RADIUS * vector_t{1, 1}),
      cv::Scalar(255, 255, 0), 1, cv::LINE_AA);
  cv::rectangle(frame,
      to_frame(PUCK_START_REGION[0] - PUCK_RADIUS * vector_t{1, 1}),
      to_frame(PUCK_START_REGION[2] + PUCK_RADIUS * vector_t{1, 1}),
      cv::Scalar(60, 221, 255), 1, cv::LINE_AA);
  cv::rectangle(frame,
      to_frame(GOAL_REGION[0] - GOAL_RADIUS * vector_t{1, 1}),
      to_frame(GOAL_REGION[2] + GOAL_RADIUS * vector_t{1, 1}),
      cv::Scalar(0, 255, 0), 1, cv::LINE_AA);

  // Draw goal region.
  cv::circle(frame, to_frame(GOAL), to_frame_dist(GOAL_RADIUS),
      cv::Scalar(0, 255, 0), 3, cv::LINE_AA);

  cv::drawMarker(frame, to_frame(BOT_START_POSITION),
      cv::Scalar(255, 255, 0), cv::MARKER_TILTED_CROSS, 20, 4, cv::LINE_AA);
  cv::drawMarker(frame, to_frame(PUCK_START_POSITION),
      cv::Scalar(60, 221, 255), cv::MARKER_TILTED_CROSS, 20, 4, cv::LINE_AA);

  // Draw ego-agent.
  cv::circle(frame, to_frame(bot_position), to_frame_dist(BOT_RADIUS),
      cv::Scalar(255, 0, 0), -1, cv::LINE_AA);
  cv::circle(frame, to_frame(bot_position), to_frame_dist(BOT_RADIUS),
      cv::Scalar(0, 0, 0), 2, cv::LINE_AA);

  // Draw puck
  cv::circle(frame, to_frame(puck_position), to_frame_dist(PUCK_RADIUS),
      cv::Scalar(25, 211, 255), -1, cv::LINE_AA);
  cv::circle(frame, to_frame(puck_position), to_frame_dist(PUCK_RADIUS),
      cv::Scalar(0, 0, 0), 2, cv::LINE_AA);

  for (auto& belief_sim : belief_sims) {
    cv::drawMarker(frame, to_frame(belief_sim.bot_position), cv::Scalar(0, 0, 255),
        cv::MARKER_CROSS, 2, 1, cv::LINE_4);

    cv::drawMarker(frame, to_frame(belief_sim.puck_position), cv::Scalar(0, 0, 255),
        cv::MARKER_CROSS, 2, 1, cv::LINE_4);
  }

  /*
  vector_t s = macro_action_start;
  for (const Action& a : macro_action) {
    vector_t e = s + vector_t(DELTA * BOT_SPEED, 0).rotated(a.orientation);
    cv::line(frame, to_frame(s), to_frame(e),
        cv::Scalar(75, 156, 0), 2, cv::LINE_AA);
    s = e;
  }
  */

  return frame;
}

}


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
// Copyright (C) 2011 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <boost/thread/thread.hpp>

// thread::id this_thread::get_id();

#include <boost/thread/thread.hpp>
#include <cstdlib>
#include <algorithm>

#include <boost/detail/lightweight_test.hpp>

#if defined BOOST_THREAD_USES_CHRONO

int main()
{
  typedef boost::chrono::system_clock Clock;
  typedef Clock::time_point time_point;
  typedef Clock::duration duration;
  boost::chrono::milliseconds ms(500);
  time_point t0 = Clock::now();
  boost::this_thread::sleep_for(ms);
  time_point t1 = Clock::now();
  boost::chrono::nanoseconds ns = (t1 - t0) - ms;
  boost::chrono::nanoseconds err = ms / 100;
  // The time slept is within 1% of 500ms
  // This test is spurious as it depends on the time the thread system switches the threads
  BOOST_TEST(std::max(ns.count(), -ns.count()) < (err+boost::chrono::milliseconds(1000)).count());
  //BOOST_TEST(std::abs(static_cast<long>(ns.count())) < (err+boost::chrono::milliseconds(1000)).count());
  return boost::report_errors();

}

#else
#error "Test not applicable: BOOST_THREAD_USES_CHRONO not defined for this platform as not supported"
#endif


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#ifndef GSPARSE_ER_EXACTER_HPP
#define GSPARSE_ER_EXACTER_HPP

#include "../Config.hpp"

#include "../Interface/EffectiveResistance.hpp"
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <iostream>
#include "Policy/ExactERJacobiCG.hpp"
namespace gSparse
{
    namespace ER
    {
        /// \ingroup EffectiveResistance
        ///
        /// This class calculates Exact value of Graph's Effective Resistance.
        /// There are many approach to this problem. Such approach is set via class Policy.
        /// Warning: The algorithm for calculating Exact Resistance is very slow.
        template <typename Policy>
        class _ExactER : public IEffectiveResistance, private Policy
        {
        public:
            using Policy::_calculateER;
            inline gSparse::COMPUTE_INFO CalculateER( gSparse::PrecisionRowMatrix & er,
                const gSparse::Graph & graph)
            {
                return _calculateER(er, graph);
            }
        };
        typedef _ExactER<Policy::ExactERJacobiCG> ExactER; //<! ExactER class to calculate effective resistance
    }
}
#endif


#include <Eigen/Dense>
#include <doubleCCD/double_subfunctions.h>
#include <doubleCCD/doubleccd.hpp>
#include <doubleCCD/exact_subtraction.hpp>
#include <fstream>
#include <iomanip>

namespace doubleccd {

// cube
cube::cube(double eps)
{
    vr[0] = Vector3d(-eps, -eps, eps), vr[1] = Vector3d(eps, -eps, eps),
    vr[2] = Vector3d(eps, eps, eps), vr[3] = Vector3d(-eps, eps, eps),
    vr[4] = Vector3d(-eps, -eps, -eps), vr[5] = Vector3d(eps, -eps, -eps),
    vr[6] = Vector3d(eps, eps, -eps), vr[7] = Vector3d(-eps, eps, -eps);
    edgeid[0] = { { 0, 1 } };
    edgeid[1] = { { 1, 2 } };
    edgeid[2] = { { 2, 3 } };
    edgeid[3] = { { 3, 0 } };
    edgeid[4] = { { 4, 5 } };
    edgeid[5] = { { 5, 6 } };
    edgeid[6] = { { 6, 7 } };
    edgeid[7] = { { 7, 4 } };
    edgeid[8] = { { 0, 4 } };
    edgeid[9] = { { 1, 5 } };
    edgeid[10] = { { 2, 6 } };
    edgeid[11] = { { 3, 7 } };
    faceid[0] = { { 0, 1, 2, 3 } };
    faceid[1] = { { 4, 7, 6, 5 } };
    faceid[2] = { { 0, 4, 5, 1 } };
    faceid[3] = { { 1, 5, 6, 2 } };
    faceid[4] = { { 3, 2, 6, 7 } };
    faceid[5] = { { 0, 3, 7, 4 } }; // orientation out
    bmax = vr[2];
    bmin = vr[4];
    epsilon = eps;
}

// get aabb corners
void get_corners(const Eigen::MatrixX3d& p, Vector3d& min, Vector3d& max)
{
    min = p.colwise().minCoeff();
    max = p.colwise().maxCoeff();
}
void get_tet_corners(
    const std::array<Vector3d, 4>& p, Vector3d& min, Vector3d& max)
{

    min = p[0];
    max = p[0];
    for (int i = 0; i < 4; i++) {
        if (min[0] > p[i][0])
            min[0] = p[i][0];
        if (min[1] > p[i][1])
            min[1] = p[i][1];
        if (min[2] > p[i][2])
            min[2] = p[i][2];

        if (max[0] < p[i][0])
            max[0] = p[i][0];
        if (max[1] < p[i][1])
            max[1] = p[i][1];
        if (max[2] < p[i][2])
            max[2] = p[i][2];
    }
}
void get_edge_coners(
    const Vector3d& e0, const Vector3d& e1, Vector3d& emin, Vector3d& emax)
{
    for (int i = 0; i < 3; i++) {
        if (e0[i] > e1[i]) {
            emin[i] = e1[i];
            emax[i] = e0[i];
        } else {
            emin[i] = e0[i];
            emax[i] = e1[i];
        }
    }
}

Vector3d get_prism_corner_double(
    const Vector3d& vertex_start,       // x0
    const Vector3d& face_vertex0_start, // x1
    const Vector3d& face_vertex1_start, // x2
    const Vector3d& face_vertex2_start, // x3
    const Vector3d& vertex_end,
    const Vector3d& face_vertex0_end,
    const Vector3d& face_vertex1_end,
    const Vector3d& face_vertex2_end,
    int i)
{
    Vector3d x0 = vertex_start, x1 = face_vertex0_start,
             x2 = face_vertex1_start, x3 = face_vertex2_start, x0b = vertex_end,
             x1b = face_vertex0_end, x2b = face_vertex1_end,
             x3b = face_vertex2_end;
    if (i == 0)
        return x0 - x1;
    if (i == 1)
        return x0 - x3;
    if (i == 2)
        return x0 - x2;
    if (i == 3)
        return x0b - x1b;
    if (i == 4)
        return x0b - x3b;
    if (i == 5)
        return x0b - x2b;

    else
        return Vector3d();
}

bool is_seg_intersect_cube(
    const double& eps, const Vector3d& e0, const Vector3d& e1)
{
    if (is_point_intersect_cube(eps, e0))
        return true;
    if (is_point_intersect_cube(eps, e1))
        return true;
    if (same_point(e0, e1))
        return false; // degenerate case: the segment is degenerated as a point
    // if intersected, must be coplanar with the edge, or intersect edge or face
    if (is_seg_intersect_cube_2d(eps, e0, e1, 0)
        && is_seg_intersect_cube_2d(eps, e0, e1, 1)
        && is_seg_intersect_cube_2d(eps, e0, e1, 2)) {

        return true;
    }

    return false;
}
// check if a 2d segment intersects 2d cube
bool is_seg_intersect_cube_2d(
    const double eps, const Vector3d& e0, const Vector3d& e1, int axis)
{
    Vector2d p0, p1, p2, p3, e0p, e1p;         // e0 and e1 projected to 2d
    projected_cube_edges(eps, p0, p1, p2, p3); // TODO move this out

    const int i1 = (axis + 1) % 3;
    const int i2 = (axis + 2) % 3;

    if (e0[i1] <= eps && e0[i1] >= -eps && e0[i2] <= eps && e0[i2] >= -eps)
        return true;
    if (e1[i1] <= eps && e1[i1] >= -eps && e1[i2] <= eps && e1[i2] >= -eps)
        return true;
    e0p = Vector2d(e0[i1], e0[i2]);
    e1p = Vector2d(e1[i1], e1[i2]);
    if (segment_segment_intersection_2d(e0p, e1p, p0, p1))
        return true; // check if segments has intersection, or if cube points
                     // p0, p1 on e0-e1
    if (segment_segment_intersection_2d(e0p, e1p, p1, p2))
        return true;
    if (segment_segment_intersection_2d(e0p, e1p, p2, p3))
        return true;
    if (segment_segment_intersection_2d(e0p, e1p, p3, p0))
        return true;

    return false;
}
void projected_cube_edges(
    const double eps, Vector2d& e0, Vector2d& e1, Vector2d& e2, Vector2d& e3)
{
    const int i1 = 0;
    const int i2 = 1;

    e0[i1] = -eps;
    e0[i2] = eps;

    e1[i1] = eps;
    e1[i2] = eps;

    e2[i1] = eps;
    e2[i2] = -eps;

    e3[i1] = -eps;
    e3[i2] = -eps;
}
bool is_point_intersect_cube(const double eps, const Vector3d& p)
{
    if (p[0] <= eps && p[0] >= -eps) {
        if (p[1] <= eps && p[1] >= -eps) {
            if (p[2] <= eps && p[2] >= -eps) {
                return true;
            }
        }
    }
    return false;
}

bool is_cube_edges_intersect_triangle(
    const cube& cb, const Vector3d& t0, const Vector3d& t1, const Vector3d& t2)
{
    // the vertices of triangle are checked before going here, the edges are
    // also checked. so, only need to check if cube edge has intersection with
    // the open triangle.

    /// if triangle degenerated as a segment or point, then
    //    no
    //                  // intersection, because before here we already check
    //                  that
    Vector3d s0, s1;
    for (int i = 0; i < 12; i++) {
        s0 = cb.vr[cb.edgeid[i][0]];
        s1 = cb.vr[cb.edgeid[i][1]];
        if (segment_triangle_intersection(s0, s1, t0, t1, t2, false)
            > 0) // return 0,1,2

            return true;
    }
    return false;
}

////////////
////////////
////////////
////////////
//////////// the following codes are for shifting vf_pairs or ee_pairs to have
/// no truncation error

// convert a array of subtraction pair to vertices
void convert_to_shifted_v(
    const std::array<std::pair<double, double>, 18>& dt, vf_pair& vs)
{
    vs.x0[0] = dt[0].first;
    vs.x0[1] = dt[1].first;
    vs.x0[2] = dt[2].first;

    vs.x0b[0] = dt[15].first;
    vs.x0b[1] = dt[16].first;
    vs.x0b[2] = dt[17].first;

    vs.x1[0] = dt[0].second;
    vs.x1[1] = dt[1].second;
    vs.x1[2] = dt[2].second;

    vs.x3[0] = dt[3].second;
    vs.x3[1] = dt[4].second;
    vs.x3[2] = dt[5].second;

    vs.x2[0] = dt[6].second;
    vs.x2[1] = dt[7].second;
    vs.x2[2] = dt[8].second;

    vs.x1b[0] = dt[9].second;
    vs.x1b[1] = dt[10].second;
    vs.x1b[2] = dt[11].second;

    vs.x3b[0] = dt[12].second;
    vs.x3b[1] = dt[13].second;
    vs.x3b[2] = dt[14].second;

    vs.x2b[0] = dt[15].second;
    vs.x2b[1] = dt[16].second;
    vs.x2b[2] = dt[17].second;
}
void convert_to_shifted_v(
    const std::array<std::pair<double, double>, 24>& dt, ee_pair& vs)
{
    vs.a0[0] = dt[0].first;
    vs.a0[1] = dt[1].first;
    vs.a0[2] = dt[2].first;

    vs.a1[0] = dt[3].first;
    vs.a1[1] = dt[4].first;
    vs.a1[2] = dt[5].first;

    vs.b0[0] = dt[0].second;
    vs.b0[1] = dt[1].second;
    vs.b0[2] = dt[2].second;

    vs.b1[0] = dt[6].second;
    vs.b1[1] = dt[7].second;
    vs.b1[2] = dt[8].second;
    //////
    vs.a0b[0] = dt[12].first;
    vs.a0b[1] = dt[13].first;
    vs.a0b[2] = dt[14].first;

    vs.a1b[0] = dt[15].first;
    vs.a1b[1] = dt[16].first;
    vs.a1b[2] = dt[17].first;

    vs.b0b[0] = dt[12].second;
    vs.b0b[1] = dt[13].second;
    vs.b0b[2] = dt[14].second;

    vs.b1b[0] = dt[18].second;
    vs.b1b[1] = dt[19].second;
    vs.b1b[2] = dt[20].second;
}

// convert vf_pairs and ee_pairs into subtraction list
void push_vers_into_subtract_pair(
    const std::vector<vf_pair>& data1,
    const std::vector<ee_pair>& data2,
    std::vector<std::pair<double, double>>& sub)
{
    sub.clear();
    sub.reserve(
        18 * data1.size() + 24 * data2.size()); // each vf_pair has 6*3, ee_pair
                                                // has 8*3 subtractions
    std::pair<double, double> temp;

    for (int j = 0; j < data1.size(); j++) {
        for (int i = 0; i < 3; i++) {
            temp.first = data1[j].x0[i];
            temp.second = data1[j].x1[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data1[j].x0[i];
            temp.second = data1[j].x3[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data1[j].x0[i];
            temp.second = data1[j].x2[i];
            sub.push_back(temp);
        }
        //
        for (int i = 0; i < 3; i++) {
            temp.first = data1[j].x0b[i];
            temp.second = data1[j].x1b[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data1[j].x0b[i];
            temp.second = data1[j].x3b[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data1[j].x0b[i];
            temp.second = data1[j].x2b[i];
            sub.push_back(temp);
        }
    }
    // ee
    for (int j = 0; j < data2.size(); j++) {
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a0[i];
            temp.second = data2[j].b0[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a1[i];
            temp.second = data2[j].b0[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a1[i];
            temp.second = data2[j].b1[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a0[i];
            temp.second = data2[j].b1[i];
            sub.push_back(temp);
        }
        //
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a0b[i];
            temp.second = data2[j].b0b[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a1b[i];
            temp.second = data2[j].b0b[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a1b[i];
            temp.second = data2[j].b1b[i];
            sub.push_back(temp);
        }
        for (int i = 0; i < 3; i++) {
            temp.first = data2[j].a0b[i];
            temp.second = data2[j].b1b[i];
            sub.push_back(temp);
        }
    }
}

// convert vf_pair into matrix
Eigen::Matrix<double, 8, 3> convert_vf_to_matrix(const vf_pair& data)
{
    Eigen::Matrix<double, 8, 3> V;
    V.row(0) = data.x0;
    V.row(1) = data.x1;
    V.row(2) = data.x2;
    V.row(3) = data.x3;
    V.row(4) = data.x0b;
    V.row(5) = data.x1b;
    V.row(6) = data.x2b;
    V.row(7) = data.x3b;
    return V;
}
// convert ee_pair into matrix
Eigen::Matrix<double, 8, 3> convert_ee_to_matrix(const ee_pair& data)
{
    Eigen::Matrix<double, 8, 3> V;
    V.row(0) = data.a0;
    V.row(1) = data.a1;
    V.row(2) = data.b0;
    V.row(3) = data.b1;
    V.row(4) = data.a0b;
    V.row(5) = data.a1b;
    V.row(6) = data.b0b;
    V.row(7) = data.b1b;
    return V;
}

// return the difference between two input pairs
// TODO this is wrong because we are not shifting back
double vf_shift_error(const vf_pair& d1, const vf_pair& d2)
{
    double err = 0;
    for (int i = 0; i < 3; i++) {
        if (fabs(d1.x0[i] - d2.x0[i]) > err)
            err = fabs(d1.x0[i] - d2.x0[i]);
        if (fabs(d1.x1[i] - d2.x1[i]) > err)
            err = fabs(d1.x1[i] - d2.x1[i]);
        if (fabs(d1.x2[i] - d2.x2[i]) > err)
            err = fabs(d1.x2[i] - d2.x2[i]);
        if (fabs(d1.x3[i] - d2.x3[i]) > err)
            err = fabs(d1.x3[i] - d2.x3[i]);

        if (fabs(d1.x0b[i] - d2.x0b[i]) > err)
            err = fabs(d1.x0b[i] - d2.x0b[i]);
        if (fabs(d1.x1b[i] - d2.x1b[i]) > err)
            err = fabs(d1.x1b[i] - d2.x1b[i]);
        if (fabs(d1.x2b[i] - d2.x2b[i]) > err)
            err = fabs(d1.x2b[i] - d2.x2b[i]);
        if (fabs(d1.x3b[i] - d2.x3b[i]) > err)
            err = fabs(d1.x3b[i] - d2.x3b[i]);
    }
    return err;
}
// return the difference between two input pairs
// TODO this is wrong because we are not shifting back
double ee_shift_error(const ee_pair& d1, const ee_pair& d2)
{
    double err = 0;
    for (int i = 0; i < 3; i++) {
        if (fabs(d1.a0[i] - d2.a0[i]) > err)
            err = fabs(d1.a0[i] - d2.a0[i]);
        if (fabs(d1.a1[i] - d2.a1[i]) > err)
            err = fabs(d1.a1[i] - d2.a1[i]);
        if (fabs(d1.b0[i] - d2.b0[i]) > err)
            err = fabs(d1.b0[i] - d2.b0[i]);
        if (fabs(d1.b1[i] - d2.b1[i]) > err)
            err = fabs(d1.b1[i] - d2.b1[i]);

        if (fabs(d1.a0b[i] - d2.a0b[i]) > err)
            err = fabs(d1.a0b[i] - d2.a0b[i]);
        if (fabs(d1.a1b[i] - d2.a1b[i]) > err)
            err = fabs(d1.a1b[i] - d2.a1b[i]);
        if (fabs(d1.b0b[i] - d2.b0b[i]) > err)
            err = fabs(d1.b0b[i] - d2.b0b[i]);
        if (fabs(d1.b1b[i] - d2.b1b[i]) > err)
            err = fabs(d1.b1b[i] - d2.b1b[i]);
    }
    return err;
}

// push the whole mesh into subtraction-pair format
void push_mesh_vers_into_sub_pair(
    const Eigen::MatrixX3d& V, std::vector<std::pair<double, double>>& sub)
{
    sub.resize(V.size());
    for (int i = 0; i < V.rows(); i++) {
        for (int j = 0; j < V.cols(); j++) {
            sub[i * 3 + j].first = V(i, j);
            sub[i * 3 + j].second = 0;
        }
    }
}
// push the whole mesh into subtraction-pair format
void push_mesh_vers_into_sub_pair(
    const Eigen::MatrixX3d& V,
    std::vector<std::pair<double, double>>& sub_x,
    std::vector<std::pair<double, double>>& sub_y,
    std::vector<std::pair<double, double>>& sub_z)
{
    sub_x.clear();
    sub_y.clear();
    sub_z.clear();
    sub_x.resize(V.rows());
    sub_y.resize(V.rows());
    sub_z.resize(V.rows());
    for (int i = 0; i < V.rows(); i++) {
        sub_x[i].first = V(i, 0);
        sub_x[i].second = 0;
        sub_y[i].first = V(i, 1);
        sub_y[i].second = 0;
        sub_z[i].first = V(i, 2);
        sub_z[i].second = 0;
    }
}

// convert the subtraction pairs into vertices
void convert_sub_pairs_to_mesh_vers(
    const std::vector<std::pair<double, double>>& sub, Eigen::MatrixX3d& V)
{
    // assert(sub.size() % 3 == 0 && V.size() == sub.size());
    V.resize(sub.size() / 3, 3);
    for (int i = 0; i < V.rows(); i++) {
        for (int j = 0; j < 3; j++) {
            V(i, j) = sub[i * 3 + j].first;
        }
    }
}

// convert the subtraction pairs into vertices
void convert_sub_pairs_to_mesh_vers(
    const std::vector<std::pair<double, double>>& sub_x,
    const std::vector<std::pair<double, double>>& sub_y,
    const std::vector<std::pair<double, double>>& sub_z,
    Eigen::MatrixX3d& V)
{
    V.resize(sub_x.size(), 3);
    for (int i = 0; i < sub_x.size(); i++) {
        V(i, 0) = sub_x[i].first;
        V(i, 1) = sub_y[i].first;
        V(i, 2) = sub_z[i].first;
    }
}

double shifted_error(
    const std::vector<std::pair<double, double>>& before,
    const std::vector<std::pair<double, double>>& after,
    const double shift)
{
    double err = 0;
    Rational s = shift;
    assert(before.size() == after.size());
    for (int i = 0; i < before.size(); i++) {
        Rational real = Rational(before[i].first) + s;
        double terr = fabs((real - Rational(after[i].first)).to_double());
        if (terr > err) {
            err = terr;
        }
        real = Rational(before[i].second) + s;
        terr = fabs((real - Rational(after[i].second)).to_double());
        if (terr > err) {
            err = terr;
        }
    }
    return err;
}

// shift whole mesh based on the bounding-box of the scene
double get_whole_mesh_shifted(
    Eigen::MatrixX3d& vertices, const Vector3d& pmin, const Vector3d& pmax)
{
    Vector3d invShift;
    return get_whole_mesh_shifted(vertices, pmin, pmax, invShift);
}

// shift whole mesh based on the bounding-box of the scene
double get_whole_mesh_shifted(
    Eigen::MatrixX3d& vertices,
    const Vector3d& pmin,
    const Vector3d& pmax,
    Vector3d& invShift)
{
    std::vector<std::pair<double, double>> box_x(1), box_y(1), box_z(1),
        whole_x, whole_y, whole_z;
    box_x[0].first = pmax[0];
    box_x[0].second = pmin[0];
    box_y[0].first = pmax[1];
    box_y[0].second = pmin[1];
    box_z[0].first = pmax[2];
    box_z[0].second = pmin[2];
    std::vector<std::pair<double, double>> bo0 = box_x, bo1 = box_y,
                                           bo2 = box_z;
    push_mesh_vers_into_sub_pair(vertices, whole_x, whole_y, whole_z);
    double k1 = perturbSubtractions(whole_x, box_x);
    double k2 = perturbSubtractions(whole_y, box_y);
    double k3 = perturbSubtractions(whole_z, box_z);
    convert_sub_pairs_to_mesh_vers(whole_x, whole_y, whole_z, vertices);
    invShift = Vector3d(-k1, -k2, -k3);
    double error = 0;
    error = std::max(error, shifted_error(bo0, box_x, k1));
    error = std::max(error, shifted_error(bo1, box_y, k2));
    error = std::max(error, shifted_error(bo2, box_z, k3));
    return error;
}

// compare the error of the whole mesh
// void compare_whole_mesh_err(const Eigen::MatrixX3d& vertices,const
// Eigen::MatrixX3d& vertices1){
//     double ex=0,ey=0,ez=0;
//     for(int i=0;i<vertices.rows();i++){
//         if(fabs(vertices(i,0)-vertices1(i,0))>ex)
//             ex=fabs(vertices(i,0)-vertices1(i,0));

//         if(fabs(vertices(i,1)-vertices1(i,1))>ey)
//             ey=fabs(vertices(i,1)-vertices1(i,1));

//         if(fabs(vertices(i,2)-vertices1(i,2))>ez)
//             ez=fabs(vertices(i,2)-vertices1(i,2));
//     }
//     std::cout<<"vertices diff x, "<<ex<<" y, "<<ey<<" z, "<<ez<<std::endl;
// }

/*x0 is the point, x1, x2, x3 is the triangle
shift the whole mesh and get the shift - back vertices*/
// double get_whole_mesh_shifted(
//     const std::vector<vf_pair>& data1,
//     const std::vector<ee_pair>& data2,
//     std::vector<vf_pair>& shift_back1,
//     std::vector<ee_pair>& shift_back2,
//     Eigen::MatrixX3d& vertices)
// {
//     std::vector<std::pair<double, double>> whole, suback;
//
//     push_vers_into_subtract_pair(data1, data2, suback);
//     int subsize = suback.size();
//     push_mesh_vers_into_sub_pair(vertices, whole);
//     // suback = sub;// this is for shift back
//     // k = displaceSubtractions_double(sub);
//     suback.insert(suback.end(), whole.begin(), whole.end());
//     perturbSubtractions(suback); // get shifted back data
//
//     shift_back1.resize(data1.size());
//
//     shift_back2.resize(data2.size());
//     int c = 0;
//     // Vector3d kvec(k, k, k);
//     int d1size = data1.size();
//     int d2size = data2.size();
//     int datasize = d1size + d2size;
//     std::array<std::pair<double, double>, 18> dtback1;
//     std::array<std::pair<double, double>, 24> dtback2;
//     for (int r = 0; r < datasize; r++) {
//         if (r < d1size) {
//             for (int i = 0; i < 18; i++) {
//                 // dt1[i] = sub[r * 18 + i];
//                 dtback1[i] = suback[r * 18 + i];
//             }
//
//             convert_to_shifted_v(dtback1, shift_back1[r]);
//         }      // r is d1size-1, sub has been read to d1size*18-1
//         else { // r is from d1size to datasize-1, sub is from d1*18
//             for (int i = 0; i < 24; i++) {
//                 // dt2[i] = sub[d1size * 18 + (r - d1size) * 24 + i];
//                 dtback2[i] = suback[d1size * 18 + (r - d1size) * 24 + i];
//             }
//
//             convert_to_shifted_v(dtback2, shift_back2[r - d1size]);
//         }
//     }
//     std::vector<std::pair<double, double>> vernew;
//     vernew.resize(vertices.size());
//     int c1 = 0;
//     for (int i = subsize; i < suback.size(); i++) {
//         vernew[c1] = suback[i];
//         c1++;
//     }
//     convert_sub_pairs_to_mesh_vers(vernew, vertices);
//     double err = 0, temerr;
//     for (int i = 0; i < d1size; i++) {
//         temerr = vf_shift_error(data1[i], shift_back1[i]);
//         if (temerr > err)
//             err = temerr;
//     }
//     for (int i = 0; i < d2size; i++) {
//         temerr = ee_shift_error(data2[i], shift_back2[i]);
//         if (temerr > err)
//             err = temerr;
//     }
//     return err;
// }
//
//// x0 is the point, x1, x2, x3 is the triangle
// double get_whole_mesh_shifted(
//     const std::vector<vf_pair>& data1,
//     const std::vector<ee_pair>& data2,
//     Eigen::MatrixX3d& vertices)
// {
//     // std::vector<std::pair<double, double>> whole, suback;
//
//     // push_vers_into_subtract_pair(data1, data2, suback);
//     // int subsize = suback.size();
//     // push_mesh_vers_into_sub_pair(vertices, whole);
//     // // suback = sub;// this is for shift back
//     // // k = displaceSubtractions_double(sub);
//     // suback.insert(suback.end(), whole.begin(), whole.end());
//     // perturbSubtractions(suback); // get shifted back data
//
//     // std::vector<std::pair<double, double>> vernew;
//     // vernew.resize(vertices.size());
//     // int c = 0;
//     // for (int i = subsize; i < suback.size(); i++) {
//     //     vernew[c] = suback[i];
//     //     c++;
//     // }
//     // assert(whole.size() == vernew.size());
//     double err = 0;
//     // for (int i = 0; i < whole.size(); i++) {
//     //     if (fabs(whole[i].first - vernew[i].first) > err) {
//     //         err = fabs(whole[i].first - vernew[i].first);
//     //     }
//     // }
//
//     // convert_sub_pairs_to_mesh_vers(vernew, vertices);
//
//     return err;
// }

// shift vertex-face pair to a far away place
double shift_vertex_face(
    const vf_pair& input_vf_pair, vf_pair& shifted_vf_pair, double& time)
{
    igl::Timer timer;
    timer.start();
    Vector3d x0 = input_vf_pair.x0, x1 = input_vf_pair.x1,
             x2 = input_vf_pair.x2, x3 = input_vf_pair.x3,
             x0b = input_vf_pair.x0b, x1b = input_vf_pair.x1b,
             x2b = input_vf_pair.x2b, x3b = input_vf_pair.x3b;

    std::vector<std::pair<double, double>> subs, subs_ori;
    subs.resize(6 * 3);
    for (int i = 0; i < 3; i++) {
        subs[3 * 0 + i].first = x0[i];
        subs[3 * 0 + i].second = x1[i];
        subs[3 * 1 + i].first = x0[i];
        subs[3 * 1 + i].second = x3[i];
        subs[3 * 2 + i].first = x0[i];
        subs[3 * 2 + i].second = x2[i];

        subs[3 * 3 + i].first = x0b[i];
        subs[3 * 3 + i].second = x1b[i];
        subs[3 * 4 + i].first = x0b[i];
        subs[3 * 4 + i].second = x3b[i];
        subs[3 * 5 + i].first = x0b[i];
        subs[3 * 5 + i].second = x2b[i];
    }
    subs_ori = subs;
    // this perturbSubtractions shift and shift - back is wrong
    // perturbSubtractions(subs);
    double k = displaceSubtractions_double(subs);
    for (int i = 0; i < 3; i++) {
        x0[i] = subs[3 * 0 + i].first;
        x1[i] = subs[3 * 0 + i].second;
        x3[i] = subs[3 * 1 + i].second;
        x2[i] = subs[3 * 2 + i].second;

        x0b[i] = subs[3 * 3 + i].first;
        x1b[i] = subs[3 * 3 + i].second;
        x3b[i] = subs[3 * 4 + i].second;
        x2b[i] = subs[3 * 5 + i].second;
    }
    shifted_vf_pair.x0 = x0;
    shifted_vf_pair.x1 = x1;
    shifted_vf_pair.x2 = x2;
    shifted_vf_pair.x3 = x3;

    shifted_vf_pair.x0b = x0b;
    shifted_vf_pair.x1b = x1b;
    shifted_vf_pair.x2b = x2b;
    shifted_vf_pair.x3b = x3b;
    timer.stop();
    time = timer.getElapsedTimeInMicroSec();
    double err = 0;
    for (int i = 0; i < subs.size(); i++) {
        double value = subs[i].first;
        double value_ori = subs_ori[i].first;
        Rational rvalue = Rational(value_ori) + Rational(k);
        double diff = (rvalue - Rational(value)).to_double();
        diff = fabs(diff);
        if (diff > err) {
            err = diff;
        }
    }
    for (int i = 0; i < subs.size(); i++) {
        double value = subs[i].second;
        double value_ori = subs_ori[i].second;
        Rational rvalue = Rational(value_ori) + Rational(k);
        double diff = (rvalue - Rational(value)).to_double();
        diff = fabs(diff);
        if (diff > err) {
            err = diff;
        }
    }

    return err;
}

// check if there are error in the subtraction, comparing with rational results
bool check_subs_err(
    std::vector<std::pair<double, double>> subs, std::string discribe)
{
    bool have_err = false;
    for (int i = 0; i < subs.size(); i++) {
        Rational a = subs[i].first;
        Rational b = subs[i].second;
        Rational rst = a - b;
        double rd = subs[i].first - subs[i].second;
        if (rst == rd) {

        } else {
            std::cout << discribe << " diff, " << std::setprecision(17) << i
                      << ", " << (rst - rd) << ", " << rst << ", " << rd
                      << std::endl;
            have_err = true;
        }
    }
    return have_err;
}

std::vector<std::pair<double, double>>
read_rational_CSV(const std::string inputFileName)
{

    std::vector<std::pair<double, double>> vs;

    vs.clear();

    std::ifstream infile;

    infile.open(inputFileName);

    // std::array<double,3> v;

    if (!infile.is_open())

    {

        std::cout << "Path Wrong!!!!" << std::endl;

        return vs;
    }
    int l = 0;

    while (infile) // there is input overload classfile

    {
        l++;
        std::string s;
        if (!getline(infile, s))
            break;
        if (s[0] != '#') {
            std::istringstream ss(s);
            std::array<std::string, 4> record;
            int c = 0;
            while (ss) {
                std::string line;
                if (!getline(ss, line, ','))
                    break;
                try {
                    record[c] = line;
                    c++;
                } catch (const std::invalid_argument e) {
                    std::cout << "NaN found in file " << inputFileName
                              << " line " << l << std::endl;
                    e.what();
                }
            }
            Rational rt;
            double x = rt.get_double(record[0], record[1]),
                   y = rt.get_double(record[2], record[3]);
            std::pair<double, double> pr;
            pr.first = x;
            pr.second = y;
            vs.push_back(pr);
        }
    }
    return vs;
    // Eigen::MatrixXd all_v(vs.size(),3);
    // for(int i=0;i<vs.size();i++){
    //     all_v(i,0)=vs[i][0];
    //   all_v(i,1)=vs[i][1];
    //     all_v(i,2)=vs[i][2];
    // }
    // if (!infile.eof()) {
    // 	std::cerr << "Could not read file " << inputFileName << "\n";
    // }
    // return all_v;
}

void print_exact_ee_pair_file(
    const ee_pair& input_ee_pair, const std::string filename)
{
    Vector3d a0 = input_ee_pair.a0, a1 = input_ee_pair.a1,
             b0 = input_ee_pair.b0, b1 = input_ee_pair.b1,
             a0b = input_ee_pair.a0b, a1b = input_ee_pair.a1b,
             b0b = input_ee_pair.b0b, b1b = input_ee_pair.b1b;

    std::vector<std::pair<double, double>> subs;
    subs.resize(8 * 3);
    for (int i = 0; i < 3; i++) {
        subs[3 * 0 + i].first = a0[i];
        subs[3 * 0 + i].second = b0[i];
        subs[3 * 1 + i].first = a1[i];
        subs[3 * 1 + i].second = b0[i];
        subs[3 * 2 + i].first = a1[i];
        subs[3 * 2 + i].second = b1[i];
        subs[3 * 3 + i].first = a0[i];
        subs[3 * 3 + i].second = b1[i];

        subs[3 * 4 + i].first = a0b[i];
        subs[3 * 4 + i].second = b0b[i];
        subs[3 * 5 + i].first = a1b[i];
        subs[3 * 5 + i].second = b0b[i];
        subs[3 * 6 + i].first = a1b[i];
        subs[3 * 6 + i].second = b1b[i];
        subs[3 * 7 + i].first = a0b[i];
        subs[3 * 7 + i].second = b1b[i];
    }
    std::ofstream fout;
    fout.open(filename);
    for (int i = 0; i < subs.size(); i++) {
        Rational n1(subs[i].first), n2(subs[i].second);
        fout << n1.get_numerator_str() << "," << n1.get_denominator_str() << ","
             << n2.get_numerator_str() << "," << n2.get_denominator_str()
             << std::endl;
    }
    fout.close();

    // read the same file and compare
    std::vector<std::pair<double, double>> readed = read_rational_CSV(filename);
    assert(readed.size() == subs.size());
    for (int i = 0; i < subs.size(); i++) {
        assert(
            readed[i].first == subs[i].first
            && readed[i].second == subs[i].second);
    }
    displaceSubtractions_double(readed);
    check_subs_err(readed, std::string("readed number"));
}

double shift_edge_edge(
    const ee_pair& input_ee_pair, ee_pair& shifted_ee_pair, double& time)
{
    igl::Timer timer;
    timer.start();
    Vector3d a0 = input_ee_pair.a0, a1 = input_ee_pair.a1,
             b0 = input_ee_pair.b0, b1 = input_ee_pair.b1,
             a0b = input_ee_pair.a0b, a1b = input_ee_pair.a1b,
             b0b = input_ee_pair.b0b, b1b = input_ee_pair.b1b;

    std::vector<std::pair<double, double>> subs, save_subs;
    subs.resize(8 * 3);
    for (int i = 0; i < 3; i++) {
        subs[3 * 0 + i].first = a0[i];
        subs[3 * 0 + i].second = b0[i];
        subs[3 * 1 + i].first = a1[i];
        subs[3 * 1 + i].second = b0[i];
        subs[3 * 2 + i].first = a1[i];
        subs[3 * 2 + i].second = b1[i];
        subs[3 * 3 + i].first = a0[i];
        subs[3 * 3 + i].second = b1[i];

        subs[3 * 4 + i].first = a0b[i];
        subs[3 * 4 + i].second = b0b[i];
        subs[3 * 5 + i].first = a1b[i];
        subs[3 * 5 + i].second = b0b[i];
        subs[3 * 6 + i].first = a1b[i];
        subs[3 * 6 + i].second = b1b[i];
        subs[3 * 7 + i].first = a0b[i];
        subs[3 * 7 + i].second = b1b[i];
    }

    save_subs = subs; // this is the value before rounding

    double k = displaceSubtractions_double(subs);
    for (int i = 0; i < 3; i++) {
        a0[i] = subs[3 * 0 + i].first;
        b0[i] = subs[3 * 0 + i].second;
        a1[i] = subs[3 * 1 + i].first;
        b1[i] = subs[3 * 2 + i].second;

        a0b[i] = subs[3 * 4 + i].first;
        b0b[i] = subs[3 * 4 + i].second;
        a1b[i] = subs[3 * 5 + i].first;
        b1b[i] = subs[3 * 6 + i].second;
    }
    shifted_ee_pair.a0 = a0;
    shifted_ee_pair.a1 = a1;
    shifted_ee_pair.b0 = b0;
    shifted_ee_pair.b1 = b1;

    shifted_ee_pair.a0b = a0b;
    shifted_ee_pair.a1b = a1b;
    shifted_ee_pair.b0b = b0b;
    shifted_ee_pair.b1b = b1b;
    timer.stop();
    time = timer.getElapsedTimeInMicroSec();
    double err = 0;
    for (int i = 0; i < subs.size(); i++) {
        double value = subs[i].first;
        double value_ori = save_subs[i].first;
        Rational rvalue = Rational(value_ori) + Rational(k);
        double diff = (rvalue - Rational(value)).to_double();
        diff = fabs(diff);
        if (diff > err) {
            err = diff;
        }
    }
    for (int i = 0; i < subs.size(); i++) {
        double value = subs[i].second;
        double value_ori = save_subs[i].second;
        Rational rvalue = Rational(value_ori) + Rational(k);
        double diff = (rvalue - Rational(value)).to_double();
        diff = fabs(diff);
        if (diff > err) {
            err = diff;
        }
    }

    return err;
}
bool have_no_truncation(const double a, const double b)
{
    Rational sub = Rational(a) - Rational(b);
    double sub_d = a - b;
    if (sub != sub_d) {
        return false;
    }
    return true;
}
// x0 is the point, x1, x2, x3 is the triangle
void get_prism_shifted_vertices_double(
    const Vector3d& x0,
    const Vector3d& x1,
    const Vector3d& x2,
    const Vector3d& x3,
    const Vector3d& x0b,
    const Vector3d& x1b,
    const Vector3d& x2b,
    const Vector3d& x3b,
    double& k,
    std::array<Vector3d, 6>& p_vertices)
{
    std::vector<std::pair<double, double>> sub;

    sub.clear();
    sub.reserve(18);
    std::pair<double, double> temp;
    for (int i = 0; i < 3; i++) {
        temp.first = x0[i];
        temp.second = x1[i];
        sub.push_back(temp);
    }
    for (int i = 0; i < 3; i++) {
        temp.first = x0[i];
        temp.second = x3[i];
        sub.push_back(temp);
    }
    for (int i = 0; i < 3; i++) {
        temp.first = x0[i];
        temp.second = x2[i];
        sub.push_back(temp);
    }
    //
    for (int i = 0; i < 3; i++) {
        temp.first = x0b[i];
        temp.second = x1b[i];
        sub.push_back(temp);
    }
    for (int i = 0; i < 3; i++) {
        temp.first = x0b[i];
        temp.second = x3b[i];
        sub.push_back(temp);
    }
    for (int i = 0; i < 3; i++) {
        temp.first = x0b[i];
        temp.second = x2b[i];
        sub.push_back(temp);
    }

    k = displaceSubtractions_double(sub);
    int c = 0;
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 3; j++) {
            p_vertices[i][j] = sub[c].first - sub[c].second;
            assert(have_no_truncation(sub[c].first, sub[c].second));
            c++;
        }
    }
}
// check if p1-p2 has truncation
bool have_no_truncation(const Vector3d& p1, const Vector3d& p2)
{
    for (int i = 0; i < 3; i++) {
        double x = p1[i] - p2[i];
        Rational xr = Rational(p1[i]) - Rational(p2[i]);
        if (xr > x || xr < x) {
            std::cout << "double, " << x << std::endl;
            std::cout << "rational, " << xr << std::endl;
            if (xr > x) {
                std::cout << "larger" << std::endl;
            }
            if (xr < x) {
                std::cout << "smaller" << std::endl;
            }
            std::cout << "diff," << xr - x << std::endl;
            std::cout << std::setprecision(17) << "pair is, " << p1[i] << ","
                      << p2[i] << std::endl;
            return false;
        }
    }
    return true;
}

// x0 is the point, x1, x2, x3 is the triangle
void prism::get_prism_vertices(
    const Vector3d& x0,
    const Vector3d& x1,
    const Vector3d& x2,
    const Vector3d& x3,
    const Vector3d& x0b,
    const Vector3d& x1b,
    const Vector3d& x2b,
    const Vector3d& x3b,
    std::array<Vector3d, 6>& p_vertices)
{
    p_vertices[0] = x0 - x1;
    p_vertices[1] = x0 - x3;
    p_vertices[2] = x0 - x2;
    p_vertices[3] = x0b - x1b;
    p_vertices[4] = x0b - x3b;
    p_vertices[5] = x0b - x2b;
    assert(have_no_truncation(x0, x1));
    assert(have_no_truncation(x0, x3));
    assert(have_no_truncation(x0, x2));
    assert(have_no_truncation(x0b, x1b));
    assert(have_no_truncation(x0b, x3b));
    assert(have_no_truncation(x0b, x2b));
}
prism::prism(
    const Vector3d& vs,
    const Vector3d& fs0,
    const Vector3d& fs1,
    const Vector3d& fs2,
    const Vector3d& ve,
    const Vector3d& fe0,
    const Vector3d& fe1,
    const Vector3d& fe2)
{

    //
    // these are the 6 vertices of the prism,right hand law
    double k = 0;
#ifdef CCD_ROUND_INPUTS
    get_prism_shifted_vertices_double(
        vs, fs0, fs1, fs2, ve, fe0, fe1, fe2, k, p_vertices);
#else
    get_prism_vertices(
        vs, fs0, fs1, fs2, ve, fe0, fe1, fe2,
        p_vertices); //  before use this we need to shift all the vertices
#endif

    std::array<int, 2> eid;

    eid[0] = 0;
    eid[1] = 1;
    prism_edge_id[0] = eid;
    eid[0] = 1;
    eid[1] = 2;
    prism_edge_id[1] = eid;
    eid[0] = 2;
    eid[1] = 0;
    prism_edge_id[2] = eid;

    eid[0] = 3;
    eid[1] = 4;
    prism_edge_id[3] = eid;
    eid[0] = 4;
    eid[1] = 5;
    prism_edge_id[4] = eid;
    eid[0] = 5;
    eid[1] = 3;
    prism_edge_id[5] = eid;

    eid[0] = 0;
    eid[1] = 3;
    prism_edge_id[6] = eid;
    eid[0] = 1;
    eid[1] = 4;
    prism_edge_id[7] = eid;
    eid[0] = 2;
    eid[1] = 5;
    prism_edge_id[8] = eid;
}
bool prism::is_triangle_degenerated(const int up_or_bottom)
{
    int pid = up_or_bottom == 0 ? 0 : 3;
    const auto to_2d = [](const Vector3d& p, int t) {
        return Vector2d(p[(t + 1) % 3], p[(t + 2) % 3]);
    };
    double r = ((p_vertices[pid] - p_vertices[pid + 1])
                    .cross(p_vertices[pid] - p_vertices[pid + 2]))
                   .norm();
    if (fabs(r) > 1e-8)
        return false;
    int ori;
    std::array<Vector2d, 3> p;
    for (int j = 0; j < 3; j++) {

        p[0] = to_2d(p_vertices[pid], j);
        p[1] = to_2d(p_vertices[pid + 1], j);
        p[2] = to_2d(p_vertices[pid + 2], j);

        ori = orient_2d(p[0], p[1], p[2]);
        if (ori != 0) {
            return false;
        }
    }
    return true;
}

void hex::get_hex_vertices(
    const Vector3d& a0,
    const Vector3d& a1,
    const Vector3d& b0,
    const Vector3d& b1,
    const Vector3d& a0b,
    const Vector3d& a1b,
    const Vector3d& b0b,
    const Vector3d& b1b,
    std::array<Vector3d, 8>& h_vertices)
{
    h_vertices[0] = a0 - b0;
    h_vertices[1] = a1 - b0;
    h_vertices[2] = a1 - b1;
    h_vertices[3] = a0 - b1;
    h_vertices[4] = a0b - b0b;
    h_vertices[5] = a1b - b0b;
    h_vertices[6] = a1b - b1b;
    h_vertices[7] = a0b - b1b;
    assert(have_no_truncation(a0, b0));
    assert(have_no_truncation(a1, b0));
    assert(have_no_truncation(a1, b1));
    assert(have_no_truncation(a0, b1));

    assert(have_no_truncation(a0b, b0b));
    assert(have_no_truncation(a1b, b0b));
    assert(have_no_truncation(a1b, b1b));
    assert(have_no_truncation(a0b, b1b));
}

void hex::get_hex_shifted_vertices_double(
    const Vector3d& a0,
    const Vector3d& a1,
    const Vector3d& b0,
    const Vector3d& b1,
    const Vector3d& a0b,
    const Vector3d& a1b,
    const Vector3d& b0b,
    const Vector3d& b1b,
    std::array<Vector3d, 8>& h_vertices)
{

    std::vector<std::pair<double, double>> subs, save_subs;
    subs.resize(8 * 3);
    for (int i = 0; i < 3; i++) {
        subs[3 * 0 + i].first = a0[i];
        subs[3 * 0 + i].second = b0[i];
        subs[3 * 1 + i].first = a1[i];
        subs[3 * 1 + i].second = b0[i];
        subs[3 * 2 + i].first = a1[i];
        subs[3 * 2 + i].second = b1[i];
        subs[3 * 3 + i].first = a0[i];
        subs[3 * 3 + i].second = b1[i];

        subs[3 * 4 + i].first = a0b[i];
        subs[3 * 4 + i].second = b0b[i];
        subs[3 * 5 + i].first = a1b[i];
        subs[3 * 5 + i].second = b0b[i];
        subs[3 * 6 + i].first = a1b[i];
        subs[3 * 6 + i].second = b1b[i];
        subs[3 * 7 + i].first = a0b[i];
        subs[3 * 7 + i].second = b1b[i];
    }

    double k = displaceSubtractions_double(subs);
    Vector3d a0_p;
    Vector3d a1_p;
    Vector3d b0_p;
    Vector3d b1_p;
    Vector3d a0b_p;
    Vector3d a1b_p;
    Vector3d b0b_p;
    Vector3d b1b_p;
    for (int i = 0; i < 3; i++) {
        a0_p[i] = subs[3 * 0 + i].first;
        b0_p[i] = subs[3 * 0 + i].second;
        a1_p[i] = subs[3 * 1 + i].first;
        b1_p[i] = subs[3 * 2 + i].second;

        a0b_p[i] = subs[3 * 4 + i].first;
        b0b_p[i] = subs[3 * 4 + i].second;
        a1b_p[i] = subs[3 * 5 + i].first;
        b1b_p[i] = subs[3 * 6 + i].second;
    }
    get_hex_vertices(
        a0_p, a1_p, b0_p, b1_p, a0b_p, a1b_p, b0b_p, b1b_p, h_vertices);
}

// a0, a1 is one edge, b0, b1 is another  edge
hex::hex(
    const Vector3d& a0,
    const Vector3d& a1,
    const Vector3d& b0,
    const Vector3d& b1,
    const Vector3d& a0b,
    const Vector3d& a1b,
    const Vector3d& b0b,
    const Vector3d& b1b)
{

#ifdef CCD_ROUND_INPUTS
    get_hex_shifted_vertices_double(
        a0, a1, b0, b1, a0b, a1b, b0b, b1b, h_vertices);
#else
    get_hex_vertices(
        a0, a1, b0, b1, a0b, a1b, b0b, b1b,
        h_vertices); // before use this we need to shift all the vertices
#endif

    std::array<int, 2> eid;

    eid[0] = 0;
    eid[1] = 1;
    hex_edge_id[0] = eid;
    eid[0] = 1;
    eid[1] = 2;
    hex_edge_id[1] = eid;
    eid[0] = 2;
    eid[1] = 3;
    hex_edge_id[2] = eid;
    eid[0] = 3;
    eid[1] = 0;
    hex_edge_id[3] = eid;

    eid[0] = 4;
    eid[1] = 5;
    hex_edge_id[4] = eid;
    eid[0] = 5;
    eid[1] = 6;
    hex_edge_id[5] = eid;
    eid[0] = 6;
    eid[1] = 7;
    hex_edge_id[6] = eid;
    eid[0] = 7;
    eid[1] = 4;
    hex_edge_id[7] = eid;

    eid[0] = 0;
    eid[1] = 4;
    hex_edge_id[8] = eid;
    eid[0] = 1;
    eid[1] = 5;
    hex_edge_id[9] = eid;
    eid[0] = 2;
    eid[1] = 6;
    hex_edge_id[10] = eid;
    eid[0] = 3;
    eid[1] = 7;
    hex_edge_id[11] = eid;
}

// the facets of the tet are all oriented to outside. check if p is inside of
// OPEN tet
bool is_point_inside_tet(const bilinear& bl, const Vector3d& p)
{

    for (int i = 0; i < 4; i++) { // facets.size()==4
        Vector3d pt1 = bl.v[bl.facets[i][0]], pt2 = bl.v[bl.facets[i][1]],
                 pt3 = bl.v[bl.facets[i][2]];
        if (orient_3d(p, pt1, pt2, pt3) >= 0) {
            return false;
        }
    }
    return true; // all the orientations are -1, then point inside
}

// we already know the bilinear is degenerated, next check which kind
int bilinear_degeneration(const bilinear& bl)
{
    bool dege1 = is_triangle_degenerated(bl.v[0], bl.v[1], bl.v[2]);
    bool dege2 = is_triangle_degenerated(bl.v[0], bl.v[2], bl.v[3]);

    if (dege1 && dege2) {
        return BI_DEGE_PLANE;
    }
    Vector3d p0, p1, p2;

    if (dege1) {
        p0 = bl.v[0];
        p1 = bl.v[2];
        p2 = bl.v[3];
    } else {
        p0 = bl.v[0];
        p1 = bl.v[1];
        p2 = bl.v[2];
    }

    Vector3d np = Vector3d::Random();
    int ori = orient_3d(np, p0, p1, p2);
    while (ori == 0) { // if coplanar, random
        np = Vector3d::Random();
        ori = orient_3d(np, p0, p1, p2);
    }
    int ori0 = orient_3d(np, bl.v[0], bl.v[1], bl.v[2]);
    int ori1 = orient_3d(np, bl.v[0], bl.v[2], bl.v[3]);
    if (ori0 * ori1 <= 0) {
        return BI_DEGE_XOR_02;
    }
    ori0 = orient_3d(np, bl.v[0], bl.v[1], bl.v[3]);
    ori1 = orient_3d(np, bl.v[3], bl.v[1], bl.v[2]);
    if (ori0 * ori1 <= 0) {
        return BI_DEGE_XOR_13;
    }
    return BI_DEGE_PLANE;
}

bool is_cube_intersect_degenerated_bilinear(
    const bilinear& bl, const cube& cube)
{
    int dege = bilinear_degeneration(bl);
    // int axis;
    bool res;
    if (dege == BI_DEGE_PLANE) {

        if (is_cube_edges_intersect_triangle(cube, bl.v[0], bl.v[1], bl.v[3]))
            return true;
        if (is_cube_edges_intersect_triangle(cube, bl.v[3], bl.v[1], bl.v[2]))
            return true;
        return false;
    } else {

        if (dege == BI_DEGE_XOR_02) { // triangle 0-1-2 and 0-2-3
            for (int i = 0; i < 12; i++) {
                res = int_seg_XOR(
                    segment_triangle_intersection(
                        cube.vr[cube.edgeid[i][0]], cube.vr[cube.edgeid[i][1]],
                        bl.v[1], bl.v[2], bl.v[0],
                        true), // CAUTION: need to be careful for the order here
                    segment_triangle_intersection(
                        cube.vr[cube.edgeid[i][0]], cube.vr[cube.edgeid[i][1]],
                        bl.v[3], bl.v[2], bl.v[0], true));
                if (res == true)
                    return true;
            }
            return false;
        }
        if (dege == BI_DEGE_XOR_13) { // triangle 0-1-2 and 0-2-3
            for (int i = 0; i < 12; i++) {
                res = int_seg_XOR(
                    segment_triangle_intersection(
                        cube.vr[cube.edgeid[i][0]], cube.vr[cube.edgeid[i][1]],
                        bl.v[0], bl.v[1], bl.v[3],
                        true), // CAUTION: need to be careful for the order here
                    segment_triangle_intersection(
                        cube.vr[cube.edgeid[i][0]], cube.vr[cube.edgeid[i][1]],
                        bl.v[2], bl.v[1], bl.v[3], true));
                if (res == true)
                    return true;
            }
            return false;
        }
    }
    std::cout << "!! THIS CANNOT HAPPEN" << std::endl;
    return false;
}
// phisign gives the pair we want to check
bool line_shoot_same_pair_tet(
    const Vector3d& p0, const Vector3d& p1, const int phisign, bilinear& bl)
{
    int fid;
    if (bl.phi_f[0] == 2)
        get_tet_phi(bl);

    if (phisign > 0) {
        if (bl.phi_f[0] > 0)
            fid = 0;
        else
            fid = 2;
    } else {
        if (bl.phi_f[1] > 0)
            fid = 0;
        else
            fid = 2;
    } // get which pair of facets to check
    // if line is parallel to the triangle, return false
    int inter0 = is_line_cut_triangle(
        p0, p1, bl.v[bl.facets[fid][0]], bl.v[bl.facets[fid][1]],
        bl.v[bl.facets[fid][2]], false);

    int inter1 = is_line_cut_triangle(
        p0, p1, bl.v[bl.facets[fid + 1][0]], bl.v[bl.facets[fid + 1][1]],
        bl.v[bl.facets[fid + 1][2]], false);

    if (inter0 == 1 && inter1 == 1)
        return true;
    return false;
}

Rational quadratic_function_value(
    const Rational& a, const Rational& b, const Rational& c, const Rational& t)
{

    if (t.get_sign() == 0) {
        return c;
    } else {
        return a * t * t + b * t + c;
    }
}
// f(t)=at^2+bt+c, when t is [t0, t1]
bool quadratic_function_rootfinder(
    const Rational& a,
    const Rational& b,
    const Rational& c,
    const Rational t0,
    const Rational t1) // t0 t1 no matter which is bigger
{
    Rational ft0, ft1;
    ft0 = quadratic_function_value(a, b, c, t0);
    ft1 = quadratic_function_value(a, b, c, t1);
    if (ft0.get_sign() == 0 || ft1.get_sign() == 0)
        return true;
    if (ft0.get_sign() != ft1.get_sign())
        return true;
    // the following are cases which signs of endpoints are same
    if (a.get_sign() == 0)
        return false;
    Rational t = -b / (2 * a);
    if (t < t1 && t > t0) {
        Rational ft
            = (4 * a * c - b * b); // actually it should be /4a. we only
                                   // care about the signs so doesnt matter
        int ftsign = a.get_sign() > 0 ? ft.get_sign() : (-1) * ft.get_sign();
        if (ft0.get_sign() != ftsign)
            return true;
    }
    return false;
}
// v0 is one point, dir is the direction,
// x0 - x3 are four points defined the shape
void get_quadratic_function(
    const Rational& v00,
    const Rational& v01,
    const Rational& v02,
    const Rational& dir0,
    const Rational& dir1,
    const Rational& dir2,
    const Vector3d x0d,
    const Vector3d x1d,
    const Vector3d x2d,
    const Vector3d x3d,
    Rational& a,
    Rational& b,
    Rational& c)
{
    Vector3r x0(x0d[0], x0d[1], x0d[2]), x1(x1d[0], x1d[1], x1d[2]),
        x2(x2d[0], x2d[1], x2d[2]), x3(x3d[0], x3d[1], x3d[2]);
    Rational x00 = x0[0], x01 = x0[1], x02 = x0[2];
    Rational x10 = x1[0], x11 = x1[1], x12 = x1[2];
    Rational x20 = x2[0], x21 = x2[1], x22 = x2[2];
    Rational x30 = x3[0], x31 = x3[1], x32 = x3[2];

    Rational x101 = x11 - x01, x100 = x10 - x00, x102 = x12 - x02,
             x201 = x21 - x01, x200 = x20 - x00, x300 = x30 - x00,
             x301 = x31 - x01, x310 = x30 - x10, x211 = x21 - x11,
             x311 = x31 - x11, x210 = x20 - x10, x202 = x22 - x02,
             x302 = x32 - x02, x212 = x22 - x12, x312 = x32 - x12;
    a = (dir0 * (x101 * x202 - x102 * x201)
         + dir1 * (-x100 * x202 + x102 * x200)
         + dir2 * (x100 * x201 - x101 * x200))
            * (dir0 * (-x201 * x302 + x202 * x301)
               + dir1 * (x200 * x302 - x202 * x300)
               + dir2 * (-x200 * x301 + x201 * x300))
        - (dir0 * (-x211 * x312 + x212 * x311)
           + dir1 * (x210 * x312 - x212 * x310)
           + dir2 * (-x210 * x311 + x211 * x310))
            * (dir0 * (x101 * x302 - x102 * x301)
               + dir1 * (-x100 * x302 + x102 * x300)
               + dir2 * (x100 * x301 - x101 * x300));
    b = ((v00 - x00) * (x101 * x202 - x102 * x201)
         + (v01 - x01) * (-x100 * x202 + x102 * x200)
         + (v02 - x02) * (x100 * x201 - x101 * x200))
            * (dir0 * (-x201 * x302 + x202 * x301)
               + dir1 * (x200 * x302 - x202 * x300)
               + dir2 * (-x200 * x301 + x201 * x300))
        + (dir0 * (x101 * x202 - x102 * x201)
           + dir1 * (-x100 * x202 + x102 * x200)
           + dir2 * (x100 * x201 - x101 * x200))
            * ((v00 - x00) * (-x201 * x302 + x202 * x301)
               + (v01 - x01) * (x200 * x302 - x202 * x300)
               + (v02 - x02) * (-x200 * x301 + x201 * x300))
        - ((v00 - x10) * (-x211 * x312 + x212 * x311)
           + (v01 - x11) * (x210 * x312 - x212 * x310)
           + (v02 - x12) * (-x210 * x311 + x211 * x310))
            * (dir0 * (x101 * x302 - x102 * x301)
               + dir1 * (-x100 * x302 + x102 * x300)
               + dir2 * (x100 * x301 - x101 * x300))
        - (dir0 * (-x211 * x312 + x212 * x311)
           + dir1 * (x210 * x312 - x212 * x310)
           + dir2 * (-x210 * x311 + x211 * x310))
            * ((v00 - x00) * (x101 * x302 - x102 * x301)
               + (v01 - x01) * (-x100 * x302 + x102 * x300)
               + (v02 - x02) * (x100 * x301 - x101 * x300));
    c = ((v00 - x00) * (x101 * x202 - x102 * x201)
         + (v01 - x01) * (-x100 * x202 + x102 * x200)
         + (v02 - x02) * (x100 * x201 - x101 * x200))
            * ((v00 - x00) * (-x201 * x302 + x202 * x301)
               + (v01 - x01) * (x200 * x302 - x202 * x300)
               + (v02 - x02) * (-x200 * x301 + x201 * x300))
        - ((v00 - x10) * (-x211 * x312 + x212 * x311)
           + (v01 - x11) * (x210 * x312 - x212 * x310)
           + (v02 - x12) * (-x210 * x311 + x211 * x310))
            * ((v00 - x00) * (x101 * x302 - x102 * x301)
               + (v01 - x01) * (-x100 * x302 + x102 * x300)
               + (v02 - x02) * (x100 * x301 - x101 * x300));
}
bool get_function_find_root(
    const bilinear& bl,
    const Vector3r& p0,
    const Vector3r& p1,
    const Rational& t0,
    const Rational& t1)
{
    if (t0 > 1 || t0 < 0)
        std::cout << "t is not right: exceed the limit" << std::endl;
    if (t1 > 1 || t1 < 0)
        std::cout << "t is not right: exceed the limit" << std::endl;
    Rational a, b, c;

    Vector3r dir = p1 - p0;
    get_quadratic_function(
        Rational(p0[0]), Rational(p0[1]), Rational(p0[2]), Rational(dir[0]),
        Rational(dir[1]), Rational(dir[2]), bl.v[0], bl.v[1], bl.v[2], bl.v[3],
        a, b, c);
    bool res = quadratic_function_rootfinder(a, b, c, t0, t1);

    return res;
}
void print_sub()
{
    // std::cout<<"time of rootfinder "<<rftime<<std::endl;
}
double root_finder_time() { return 0; }
bool rootfinder(
    const bilinear& bl,
    const Vector3d& p0d,
    const Vector3d& p1d,
    const bool p0in,
    const bool p1in,
    const int pairid)
{

    Vector3r p0(p0d[0], p0d[1], p0d[2]), p1(p1d[0], p1d[1], p1d[2]);

    if (p0in && p1in) {
        // t0=0, t1=1
        return get_function_find_root(bl, p0, p1, Rational(0), Rational(1));
    }
    int fid = 2 * pairid; // it should be 0 or 2
    Rational t;
    if (p0in) {
        bool res1 = seg_triangle_inter_return_t(
            p0d, p1d, bl.v[bl.facets[fid][0]], bl.v[bl.facets[fid][1]],
            bl.v[bl.facets[fid][2]], t);
        if (!res1) {
            res1 = seg_triangle_inter_return_t(
                p0d, p1d, bl.v[bl.facets[fid + 1][0]],
                bl.v[bl.facets[fid + 1][1]], bl.v[bl.facets[fid + 1][2]], t);
        }
        if (!res1)
            return false; // means not really intersected
        // we got t
        return get_function_find_root(bl, p0, p1, 0, t);
    }
    if (p1in) { // change the order of input to get t just because we want
                // domain to be [0, t]
        bool res1 = seg_triangle_inter_return_t( // here get n1, d1, n2, d2
            p1d, p0d, bl.v[bl.facets[fid][0]], bl.v[bl.facets[fid][1]],
            bl.v[bl.facets[fid][2]], t);
        if (!res1) {
            res1 = seg_triangle_inter_return_t(
                p1d, p0d, bl.v[bl.facets[fid + 1][0]],
                bl.v[bl.facets[fid + 1][1]], bl.v[bl.facets[fid + 1][2]], t);
        }
        if (!res1)
            return false; // means not really intersected
        // we got t
        return get_function_find_root(bl, p1, p0, 0, t);
    }

    Rational t1;
    bool res1 = seg_triangle_inter_return_t(
        p0d, p1d, bl.v[bl.facets[fid][0]], bl.v[bl.facets[fid][1]],
        bl.v[bl.facets[fid][2]], t);
    if (!res1)
        return false; // means not really intersected
    bool res2 = seg_triangle_inter_return_t(
        p0d, p1d, bl.v[bl.facets[fid + 1][0]], bl.v[bl.facets[fid + 1][1]],
        bl.v[bl.facets[fid + 1][2]], t1);
    if (!res2)
        return false; // means not really intersected
    // we got t, t1
    return get_function_find_root(bl, p0, p1, t, t1);
}
// segment intersect two opposite faces not included. compare phi, then use root
// finder
bool is_seg_intersect_not_degenerated_bilinear(
    bilinear& bl,
    const Vector3d& p0,
    const Vector3d& p1,
    const bool pin0,
    const bool pin1)
{
    // first compare phi, if phis are different, intersected;
    // then check if the line intersect two opposite facets of bilinear, if so,
    // use rootfinder, else, not intersected

    if (pin0 && pin1) { // two points are all inside

        Rational phi0 = phi(p0, bl.v);
        Rational phi1 = phi(p1, bl.v);
        if (phi0 == 0 || phi1 == 0 || phi0.get_sign() != phi1.get_sign())
            return true;
        if (line_shoot_same_pair_tet(p0, p1, phi1.get_sign(), bl)) {
            if (phi1.get_sign() == bl.phi_f[0]) {

                bool rf = rootfinder(bl, p0, p1, pin0, pin1, 0);

                return rf;
            }

            else {

                bool rf = rootfinder(bl, p0, p1, pin0, pin1, 1);

                return rf;
            }

        }

        else
            return false; // if the phis are the same, and shoot same pair, need
                          // to use rootfinder
    }
    if (pin0) {
        Rational phi0 = phi(p0, bl.v);
        if (phi0 == 0)
            return true;
        int hitpair = -1;

        for (int i = 0; i < 4; i++) {
            if (segment_triangle_intersection( // 0,1,2,3. 1,2,3 are all
                                               // intersected
                    p0, p1, bl.v[bl.facets[i][0]], bl.v[bl.facets[i][1]],
                    bl.v[bl.facets[i][2]], false)
                > 0) {
                if (i < 2)
                    hitpair = 0;
                else
                    hitpair = 1;
                break;
            }
        }
        if (bl.phi_f[0] == 2)
            get_tet_phi(bl);
        if (hitpair == -1)
            return false;
        if (phi0.get_sign()
            != bl.phi_f[hitpair]) { // if different, intersected, if same,
                                    // extend; if shoot same, rootfinder, if
                                    // shoot diff, false
            return true;
        }

        else {
            if (line_shoot_same_pair_tet(p0, p1, phi0.get_sign(), bl)) {
                if (phi0.get_sign() == bl.phi_f[0]) {

                    bool rf = rootfinder(bl, p0, p1, pin0, pin1, 0);

                    return rf;
                }

                else {

                    bool rf = rootfinder(bl, p0, p1, pin0, pin1, 1);

                    return rf;
                }

            }

            else
                return false; // if the phis are the same, and shoot same pair,
                              // need to use rootfinder
        }
    }
    if (pin1) {
        Rational phi1 = phi(p1, bl.v);
        if (phi1 == 0)
            return true;
        int hitpair = -1;
        for (int i = 0; i < 4; i++) {
            if (segment_triangle_intersection( // 0,1,2,3. 1,2,3 are all
                                               // intersected
                    p0, p1, bl.v[bl.facets[i][0]], bl.v[bl.facets[i][1]],
                    bl.v[bl.facets[i][2]], false)
                > 0) {
                if (i < 2)
                    hitpair = 0;
                else
                    hitpair = 1;
                break;
            }
        }
        if (bl.phi_f[0] == 2)
            get_tet_phi(bl);
        if (hitpair == -1)
            return false; // parallel , should be impossible
        if (phi1.get_sign() != bl.phi_f[hitpair]) {
            return true;
        }

        else {
            if (line_shoot_same_pair_tet(p0, p1, phi1.get_sign(), bl)) {
                if (phi1.get_sign() == bl.phi_f[0]) {

                    bool rf = rootfinder(bl, p0, p1, pin0, pin1, 0);

                    return rf;
                }

                else {

                    bool rf = rootfinder(bl, p0, p1, pin0, pin1, 1);

                    return rf;
                }

            } else
                return false; // if the phis are the same, and shoot same pair,
                              // need to use rootfinder
        }
    }
    if (!pin0
        && !pin1) { // not intersect tet (false), or intersect same side(root
                    // finder) or intersect diff side(checked before)

        if (line_shoot_same_pair_tet(p0, p1, 1, bl)) {
            if (1 == bl.phi_f[0]) {

                bool rf = rootfinder(bl, p0, p1, pin0, pin1, 0);

                return rf;
            }

            else {

                bool rf = rootfinder(bl, p0, p1, pin0, pin1, 1);

                return rf;
            }

        }

        else if (line_shoot_same_pair_tet(p0, p1, -1, bl)) {
            if (-1 == bl.phi_f[0]) {

                bool rf = rootfinder(bl, p0, p1, pin0, pin1, 0);

                return rf;
            }

            else {

                bool rf = rootfinder(bl, p0, p1, pin0, pin1, 1);

                return rf;
            }
        }
        return false; // if the phis are the same, and shoot same pair,
                      // need to use rootfinder
    }
    std::cout
        << " it cannot happen here in is_seg_intersect_not_degenerated_bilinear"
        << std::endl;
    return false;
}

bool is_cube_edge_intersect_bilinear(
    bilinear& bl, const cube& cb, const std::array<bool, 8>& pin)
{
    if (bl.is_degenerated)
        return false; // we already checked degenerated cases
    for (int i = 0; i < 12; i++) {
        if (is_seg_intersect_not_degenerated_bilinear(
                bl, cb.vr[cb.edgeid[i][0]], cb.vr[cb.edgeid[i][1]],
                pin[cb.edgeid[i][0]], pin[cb.edgeid[i][1]]))
            return true;
    }
    return false;
}
// vin is true, this vertex has intersection with open tet
// if tet is degenerated, just tell us if cube is intersected with the shape
bool is_cube_intersect_tet_opposite_faces(
    const bilinear& bl,
    const Vector3d& pmin,
    const Vector3d& pmax,
    const cube& cube,
    std::array<bool, 8>& vin,
    bool& cube_inter_tet)
{

    cube_inter_tet = false;
    if (!bl.is_degenerated) {
        for (int i = 0; i < 8; i++) {
            vin[i] = false;

            if (is_point_inside_tet(bl, cube.vr[i])) {
                cube_inter_tet = true;
                vin[i] = true;
            }
        }
    } else {

        bool rst = is_cube_intersect_degenerated_bilinear(bl, cube);

        return rst;
    }

    bool side1 = false;
    bool side2 = false;
    Vector3d vmin, vmax;
    for (int i = 0; i < 12; i++) {

        if (vin[cube.edgeid[i][0]]
            && vin[cube.edgeid[i][1]]) { // if two vertices are all inside, it
                                         // can not cut any edge
            cube_inter_tet = true;
            continue;
        }
        get_edge_coners(
            cube.vr[cube.edgeid[i][0]], cube.vr[cube.edgeid[i][1]], vmin, vmax);
        if (!box_box_intersection(pmin, pmax, vmin, vmax)) {
            continue;
        }
        for (int j = 0; j < 4; j++) {

            int inter = segment_triangle_intersection(
                cube.vr[cube.edgeid[i][0]], cube.vr[cube.edgeid[i][1]],
                bl.v[bl.facets[j][0]], bl.v[bl.facets[j][1]],
                bl.v[bl.facets[j][2]], false);

            if (inter > 0) {
                cube_inter_tet = true;
                if (j == 0 || j == 1)
                    side1 = true;
                if (j == 2 || j == 3)
                    side2 = true;
                if (side1 && side2)
                    return true;
            }
        }
    }
    if (side1 && side2)
        return true;
    return false;
}
bool cube_discrete_bilinear_intersection(
    const cube& cb, const bilinear& bl, int n)
{
    Vector3d s0, s1;
    for (int i = 0; i < 12; i++) {
        s0 = cb.vr[cb.edgeid[i][0]];
        s1 = cb.vr[cb.edgeid[i][1]];
        if (seg_discrete_bilinear_intersection(bl, n, s0, s1)) // return 0,1,2

            return true;
    }
    return false;
}

} // namespace doubleccd