#include "misc.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-register"
#include <boost/regex.hpp>
#pragma GCC diagnostic pop

namespace spn {
	float AngleValueNL(const Vec2& dir, const Vec2& dirA) {
		float d0 = dir.dot(dirA);
		if(dirA.cw(dir) <= -1e-6f)
			return d0+1 + 2;
		return 2.f-(d0+1);
	}
	RadF AngleValue(const Vec2& dir) {
		float ac0 = std::acos(std::max(-1.f, std::min(1.f,dir.y)));
		if(dir.x >= 1e-6f)
			return RadF(RadF::OneRotationAng - ac0);
		return RadF(ac0);
	}
	float AngleLerpValueDiff(float ang0, float ang1, const float oneloop) {
		auto diff = ang1 - ang0;
		const float LHalf = oneloop/2;
		if(diff >= LHalf)
			diff = -oneloop + diff;
		else if(diff < -LHalf)
			diff = oneloop + diff;
		return diff;
	}
	Vec2 VectorFromAngle(const RadF& ang) {
		const auto angv = ang.get();
		return Vec2(-std::sin(angv), std::cos(angv));
	}

	const char* AngleInfo<Degree_t>::name = "degree";
	const char* AngleInfo<Degree_t>::name_short = "deg";
	const char* AngleInfo<Radian_t>::name = "radian";
	const char* AngleInfo<Radian_t>::name_short = "rad";

	float CramerDet(const Vec3& v0, const Vec3& v1, const Vec3& v2) {
		return v0.cross(v1).dot(v2);
	}
	float CramerDet(const Vec2& v0, const Vec2& v1) {
		return v0.x * v1.y - v1.x * v0.y;
	}

	Vec3 CramersRule(const Vec3& v0, const Vec3& v1, const Vec3& v2, const Vec3& a0, float detInv) {
		return Vec3(CramerDet(a0,v1,v2) * detInv,
					CramerDet(v0,a0,v2) * detInv,
					CramerDet(v0,v1,a0) * detInv);
	}
	Vec2 CramersRule(const Vec2& v0, const Vec2& v1, const Vec2& a0, float detInv) {
		return Vec2(CramerDet(a0, v1) * detInv,
					CramerDet(v0, a0) * detInv);
	}
	void GramSchmidtOrth(Vec3& v0, Vec3& v1, Vec3& v2) {
		Assert(Throw, v0.normalize() >= 1e-6f)
		v1 -= v0 * v0.dot(v1);
		Assert(Throw, v1.normalize() >= 1e-6f)
		v2 -= v0*v0.dot(v2) + v1*v1.dot(v2);
		Assert(Throw, v2.normalize() >= 1e-6f)
	}

	// ------------ YawPitchDist ------------
	// 原点からのYawとPitch,Distanceを算出。Rollは無視
	YawPitchDist YawPitchDist::FromPos(const spn::Vec3& pos) {
		YawPitchDist ypd;
		// Distance
		spn::Vec3 v(pos);
		ypd.distance = v.length();
		v /= ypd.distance;

		// Yaw
		spn::Vec3 xzvec(v.x, 0, v.z);
		if(xzvec.len_sq() < 1e-6f)
			ypd.yaw.set(0);
		else {
			xzvec.normalize();
			float ac = std::acos(Saturate(xzvec.z, 1.f));
			if(xzvec.x < 0)
				ac = 2*spn::Pi<float> - ac;
			ypd.yaw.set(ac);
		}

		// Pitch
		constexpr float h = AngleInfo<Radian_t>::onerotation<float> / 2.f;
		ypd.pitch.set(Saturate(std::asin(Saturate(v.y, 1.f)), -h, h));
		return ypd;
	}
	std::pair<spn::Vec3, spn::Quat> YawPitchDist::toOffsetRot() const {
		// Z軸をYaw/Pitch/Roll傾けた方向に対してDist距離進んだ場所がカメラの位置
		// カメラの方向は変換済みZ軸と逆
		spn::AQuat q = spn::AQuat::RotationYPR(yaw, pitch, RadF(0));
		spn::AVec3 z = q.getDir();
		spn::Vec3 pos(z*distance);
		spn::Vec3 vd = -(z*distance).normalization();
		spn::Quat rot(spn::Quat::LookAt(vd, spn::Vec3(0,1,0)));
		return std::make_pair(pos, rot);
	}
	std::ostream& operator << (std::ostream& os, const YawPitchDist& ypd) {
		os << boost::format("YPD: yaw=%1% pitch=%2% dist=%3%") % ypd.yaw % ypd.pitch % ypd.distance;
		return os;
	}

	// ------------ AffineParts ------------
	AffineParts AffineParts::Decomp(const AMat43& m) {
		AMat33 tm;
		AffineParts ap;
		// オフセットは4行目をそのまま抽出
		ap.offset = m.getRow(3);
		for(int i=0 ; i<3 ; i++) {
			auto r = m.getRow(i);
			ap.scale.m[i] = r.length();
			tm.getRow(i) = r * Rcp22Bit(ap.scale.m[i]);
		}
		// 掌性チェックして、左手系でなければX軸を反転
		auto cv = tm.getRow(1).cross(tm.getRow(2));
		if(tm.getRow(0).dot(cv) <= 0) {
			tm.getRow(0) *= -1;
			ap.scale.x *= -1;
		}

		ap.rotation = Quat::FromAxis(tm.getRow(0), tm.getRow(1), tm.getRow(2));
		return ap;
	}
	std::string AddLineNumber(const std::string& src, int numOffset, int viewNum, bool bPrevLR, bool bPostLR) {
		std::string::size_type pos[2] = {};
		std::stringstream ss;
		if(bPrevLR)
			ss << std::endl;
		int lnum = numOffset;
		for(;;) {
			if(lnum >= viewNum)
				ss << (boost::format("%5d:\t") % lnum);
			else
				ss << "      \t";
			++lnum;
			pos[1] = src.find_first_of('\n', pos[0]);
			if(pos[1] == std::string::npos) {
				ss.write(&src[pos[0]], src.length()-pos[0]);
				break;
			}
			ss.write(&src[pos[0]], pos[1]-pos[0]);
			ss << std::endl;
			pos[0] = pos[1]+1;
		}
		if(bPostLR)
			ss << std::endl;
		return ss.str();
	}
	namespace {
		// 行分離用Regex
		const static boost::regex re_line("^(.+?)(\\n|$)");
		template <class CB>
		void RegexFindCallback(const std::string& str, const boost::regex& re, CB cb) {
			boost::smatch m, m2;
			int lnum = 0;
			auto itrB = str.cbegin(),
				itrE = str.cend();
			// まず行で分ける
			while(boost::regex_search(itrB, itrE, m, re_line)) {
				// ユーザー指定のRegexでチェック
				if(boost::regex_search(m[1].first, m[1].second, m2, re)) {
					if(!cb(lnum, m))
						break;
				}
				itrB = m[0].second;
				++lnum;
			}
		}
	}
	OPLinePos RegexFindFirst(const std::string& str, const boost::regex& re) {
		OPLinePos ret;
		RegexFindCallback(str, re, [&ret](int lnum, const boost::smatch& m)->bool{
			ret = std::make_pair(lnum, m[0].first);
			return false;
		});
		return ret;
	}
	OPLinePos RegexFindLast(const std::string& str, const boost::regex& re) {
		OPLinePos ret;
		RegexFindCallback(str, re, [&ret](int lnum, const boost::smatch& m)->bool{
			ret = std::make_pair(lnum, m[0].first);
			return true;
		});
		return ret;
	}
	Vec2 MixVector(const float (&cf)[3], const Vec2& p0, const Vec2& p1, const Vec2& p2) {
		return p0 * cf[0] +
				p1 * cf[1] +
				p2 * cf[2];
	}
	void BarycentricCoord(float ret[3], const Vec2& p0, const Vec2& p1, const Vec2& p2, const Vec2& pos) {
		Vec2 pt0 = p0 - p2,
			pt1 = p1 - p2,
			ptp = pos - p2;

		spn::Mat22 m;
		m.setColumn(0, pt0);
		m.setColumn(1, pt1);
		float det = m.calcDeterminant();

		m.setColumn(0, ptp);
		ret[0] = m.calcDeterminant();

		m.setColumn(0, pt0);
		m.setColumn(1, ptp);
		ret[1] = m.calcDeterminant();
		if(det != 0) {
			ret[0] /= det;
			ret[1] /= det;
		}
		ret[2] = 1.f - ret[0] - ret[1];
	}
}


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Algorithms
#include "pinocchio/algorithm/geometry.hpp"
// Parsers
#include "pinocchio/parsers/urdf.hpp"
// Spatial
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
    // allows us to define a more familiar translation and rotation and convert to SE3 as needed for GeometryObject
    // i.e:
    //// give the boxes defined transforms          ROTATION QUATERNION               TRANSLATION VECTOR
    // pinocchio::fcl::Transform3f tf1 (makeQuat (cos (pi/8), 0, 0, sin (pi/8)), pinocchio::fcl::Vec3f (-2, 1, .5));
    // SE3 placement = pinocchio::toPinocchioSE3(tf1)

// Standard
#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/shared_ptr.hpp>

using namespace pinocchio;

fcl::Quaternion3f makeQuat(double w, double x, double y, double z){
  pinocchio::fcl::Quaternion3f q;
  q.w() = w;
  q.x() = x;
  q.y() = y;
  q.z() = z;
  return q;
}

int main(int argc, char ** argv){
std::vector<std::string> getBodiesList();
  static double pi = M_PI;
  fcl::Transform3f tf1 (makeQuat (0, 0, 0, 0), pinocchio::fcl::Vec3f (1.5, 1.5, 1.5));
  SE3 placement = pinocchio::toPinocchioSE3(tf1);

// This script is a portion of: stack-of-tasks/pinocchio/unittest/geom.cpp
// this script creates a simple box

  using namespace pinocchio;
  Model model;
  GeometryModel geomModel;

// define a JointIndex
  Model::JointIndex idx;
//--------------- FIRST
// addJoint is rtype JointIndexoint has two variations based on # of args
  	// This version has infinite bounds, the other is given finite bounds
  // it is fed:
	// 1 - index of parent joint
  	// 2 - the joint model -> these live in parsers/urdf/model.hxx
  	// 3 - joint placement = placement of the joint inside its parent joint
  	// 4 - name of the joint
  	// OPTIONALLY:
  	// 5, 6, 7, 8 - max_effort, max_velocity, min_config, max_config
  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar1_joint");
// addJointFrame rtype is int, which is index of new frame; -1 returned if error
  // it is fed:
  	// 1 - index of the joint
  	// 2 - frame index, which is the index of parent frame. Default is -1
  		// when negative, the parent frame is the frame of the parent joint
  model.addJointFrame(idx);
// appendBodyToJoint rtype is void, but it appends a body to a given joint of a kinematic tree
  // it is fed:
  	// 1 - index of the supporting joint
  	// 2 - Y spatial inertia of body
  	// 3 - body_placement , relative placement of body regarding the parent joint
  		// default is identity
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
// addBodyFrame rtype int, index of the body frame, -1 if error
  // it is fed:
  	// 1 - name of body
  	// 2 - index of parent joint
  	// 3 - body_placement (see above)
  	// 4 - previousFrame , index of parent frame. Default is -1
  		// if negative, the parent frame is the frame of the parent joint
  model.addBodyFrame("planar1_body", idx, SE3::Identity());
//---------------- END FIRST

//---------------- SECOND
  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
  model.addJointFrame(idx);
  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame("planar2_body", idx, SE3::Identity());
//---------------- END SECOND

  // fcl::Box is a data type defined in fcl, and we are giving sides 1x1x1
  // sample is the name of the object type boost::shared_ptr with an argument of type fcl::Box
  boost::shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1));
  // we create object of frameindex and then get body1ID
  Model::FrameIndex body_id_1 = model.getBodyId("planar1_body");
  Model::JointIndex joint_parent_1 = model.frames[body_id_1].parent;
  // addGeometryObject rtype GeomIndex, which is ..
  // it adds a geometry object to a geometry model and the second argument sets a parent joint
  	//it has two versions which are fed:
  		// 1 - GeometryObject
  			// GeomModel::GeometryObject, lives in pinocchio/src/multibody/fcl.hpp (INCLUDED WITH IFNDEF IN multibody/geometry.hpp)
        //GeometryObject(name, parentFrame, parentJoint, shared_ptr<fcl::CollisionGeometry>(collision object), placementSE3, meshPath, meshScale,...)
        // HERE:
          // name = 'ff1_collision_object'
          // parentFrame = model.getBodyId("planar1_body")
          // parentJoint = joint_parent_1
          // shared_ptr<fcl::CollisionGeometry> = sample
          // placement = SE3::Identity()
  		// Optionally: 2 - corresponding model, which is used to assert the attributes of the object
  Model::JointIndex idx_geom1 = geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
                                                                           model.getBodyId("planar1_body"),joint_parent_1,
                                                                           sample,placement, "", Eigen::Vector3d::Ones())
                                                            );
  // since the version of addGEometryObject is version 1, we must set a parent joint
  geomModel.geometryObjects[idx_geom1].parentJoint = model.frames[body_id_1].parent;

  boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1));
  Model::FrameIndex body_id_2 = model.getBodyId("planar2_body");
  Model::JointIndex joint_parent_2 = model.frames[body_id_2].parent;
  // Note that this is the second version which sets the parent joint
  Model::JointIndex idx_geom2 = geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
                                                                           model.getBodyId("planar2_body"),joint_parent_2,
                                                                           sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()),
                                                            model);

// self explanatory. Defined in src/multibody/geometry.cpp
  // note that collision pairs b/w objects with same parent joint not added
  geomModel.addAllCollisionPairs();
  pinocchio::Data data(model);
  //geomData is where collision computations will be done
  pinocchio::GeometryData geomData(geomModel);

  int j, i;
  pinocchio::PairIndex PairId;
  pinocchio::fcl::CollisionResult result;
  pinocchio::fcl::DistanceResult dresult;

  //CollisionPair created collision pair from two collision object indexes, note index 1 < index 2, otherwise constructor will flip them

  std::cout << "------ Model ------ " << std::endl;
  std::cout << model;
  std::cout << "------ Geom ------ " << std::endl;
  std::cout << geomModel;
  std::cout << "------ DataGeom ------ " << std::endl;
  std::cout << geomData;

  Eigen::VectorXd q(model.nq);
  q <<  0, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);

  pinocchio::computeCollisions(model,data,geomModel,geomData,q);

  std::cout << "------------Collision Results: " << std::endl;
  for(j=0; j<geomData.collisionResults.size(); j++)
  {
    // computeCollisions sets collisionPairIndex to the first colliding pair
      PairId = geomData.collisionPairIndex;
      std::cout << "PairIndex PairId: " << PairId << std::endl;
    //We iterate through the collisionResults vector so we can determine if each pair is in collision
      result = geomData.collisionResults[j];
      std::vector<pinocchio::fcl::Contact> contacts;
      result.getContacts(contacts);
      std::cout << contacts.size() << " contacts found" << std::endl;
      for(i=0; i<contacts.size(); i++)
      {
        std::cout << "position: " << contacts[i].pos << std::endl;
      }
  }

  pinocchio::computeDistances(model,data,geomModel,geomData,q);

  std::cout << "------------Distance Query Results: " << std::endl;
  for(j=0; j<geomData.distanceResults.size(); j++)
  {
    //We iterate through the distanceResults vector so we can determine each pairs distance
      dresult = geomData.distanceResults[j];
      // We can print the collision objects, but this is quite useless
        // Unsure how to go from collision object, which is an address to the name of the link it represents
      std::cout << "collision object 1: " << dresult.o1 << std::endl;
      std::cout << "collision object 2: " << dresult.o2 << std::endl;
      std::cout << "result.min_distance: " << dresult.min_distance << std::endl;
      //std::cout << "result.nearest_points[1]" << result.nearest_points[1] << std::endl;
  }

  q <<  2, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);

  std::cout << "------------Collision Results2: " << std::endl;
  for(j=0; j<geomData.collisionResults.size(); j++)
  {
    // computeCollisions sets collisionPairIndex to the first colliding pair
      PairId = geomData.collisionPairIndex;
      std::cout << "PairIndex PairId: " << PairId << std::endl;
    //We iterate through the collisionResults vector so we can determine if each pair is in collision
      result = geomData.collisionResults[j];
      std::vector<pinocchio::fcl::Contact> contacts;
      result.getContacts(contacts);
      std::cout << contacts.size() << " contacts found" << std::endl;
      for(i=0; i<contacts.size(); i++)
      {
        std::cout << "position: " << contacts[i].pos << std::endl;
      }
  }
  pinocchio::computeDistances(model,data,geomModel,geomData,q);

  std::cout << "------------Distance Query Results2: " << std::endl;
  for(j=0; j<geomData.distanceResults.size(); j++)
  {
    //We iterate through the distanceResults vector so we can determine each pairs distance
      dresult = geomData.distanceResults[j];
      // We can print the collision objects, but this is quite useless
        // Unsure how to go from collision object, which is an address to the name of the link it represents
      std::cout << "collision object 1: " << dresult.o1 << std::endl;
      std::cout << "collision object 2: " << dresult.o2 << std::endl;
      std::cout << "result.min_distance: " << dresult.min_distance << std::endl;
      //std::cout << "result.nearest_points[1]" << result.nearest_points[1] << std::endl;
  }

  q <<  0.99, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);

  std::cout << "------------Collision Results:3 " << std::endl;
  for(j=0; j<geomData.collisionResults.size(); j++)
  {
    // computeCollisions sets collisionPairIndex to the first colliding pair
      PairId = geomData.collisionPairIndex;
      std::cout << "PairIndex PairId: " << PairId << std::endl;
    //We iterate through the collisionResults vector so we can determine if each pair is in collision
      result = geomData.collisionResults[j];
      std::vector<pinocchio::fcl::Contact> contacts;
      result.getContacts(contacts);
      std::cout << contacts.size() << " contacts found" << std::endl;
      for(i=0; i<contacts.size(); i++)
      {
        std::cout << "position: " << contacts[i].pos << std::endl;
      }
  }
  pinocchio::computeDistances(model,data,geomModel,geomData,q);

  std::cout << "------------Distance Query Results3: " << std::endl;
  for(j=0; j<geomData.distanceResults.size(); j++)
  {
    //We iterate through the distanceResults vector so we can determine each pairs distance
      dresult = geomData.distanceResults[j];
      // We can print the collision objects, but this is quite useless
        // Unsure how to go from collision object, which is an address to the name of the link it represents
      std::cout << "collision object 1: " << dresult.o1 << std::endl;
      std::cout << "collision object 2: " << dresult.o2 << std::endl;
      std::cout << "result.min_distance: " << dresult.min_distance << std::endl;
      //std::cout << "result.nearest_points[1]" << result.nearest_points[1] << std::endl;
  }

  q <<  1.01, 0, 1, 0,
        0, 0, 1, 0 ;

  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);

  std::cout << "------------Collision Results4: " << std::endl;
  for(j=0; j<geomData.collisionResults.size(); j++)
  {
    // computeCollisions sets collisionPairIndex to the first colliding pair
      PairId = geomData.collisionPairIndex;
      std::cout << "PairIndex PairId: " << PairId << std::endl;
    //We iterate through the collisionResults vector so we can determine if each pair is in collision
      result = geomData.collisionResults[j];
      std::vector<pinocchio::fcl::Contact> contacts;
      result.getContacts(contacts);
      std::cout << contacts.size() << " contacts found" << std::endl;
      for(i=0; i<contacts.size(); i++)
      {
        std::cout << "position: " << contacts[i].pos << std::endl;
      }
  }
  pinocchio::computeDistances(model,data,geomModel,geomData,q);

  std::cout << "------------Distance Query Results4: " << std::endl;
  for(j=0; j<geomData.distanceResults.size(); j++)
  {
    //We iterate through the distanceResults vector so we can determine each pairs distance
      dresult = geomData.distanceResults[j];
      // We can print the collision objects, but this is quite useless
        // Unsure how to go from collision object, which is an address to the name of the link it represents
      std::cout << "collision object 1: " << dresult.o1 << std::endl;
      std::cout << "collision object 2: " << dresult.o2 << std::endl;
      std::cout << "result.min_distance: " << dresult.min_distance << std::endl;
      //std::cout << "result.nearest_points[1]" << result.nearest_points[1] << std::endl;
  }

}


#include <algorithm>
#include <array>
#include <bits/c++config.h>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <exception>
#include <filesystem>
#include <initializer_list>
#include <iostream>
#include <memory>
#include <numeric>
#include <ostream>
#include <random>
#include <set>
#include <sstream>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

namespace fs = std::filesystem;
using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::Vector2i;
using Eigen::aligned_allocator;
using RowMatrixXd =
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using RowMatrixXi =
    Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

auto readData(const std::string& inputPathString) {
  fs::path inputPath(inputPathString);
  std::set<std::string> validExts{{".jpg", ".png", ".jpeg", ".tiff"}};
  std::vector<cv::Mat> X;
  std::vector<int> y;
  for (const auto &dirEntry : fs::directory_iterator(inputPath)) {
    if (!(dirEntry.is_regular_file() &&
          validExts.find(dirEntry.path().extension().string()) != validExts.end()))
      continue;
    fs::path imagePath = dirEntry.path();
    cv::Mat image = cv::imread(imagePath.string());
    if (image.channels() > 1) {
      // Convert to grayscale.
      cv::Mat imageGray;
      cv::cvtColor(image, imageGray, cv::COLOR_BGR2GRAY);
      X.push_back(imageGray);
    }
    else
      X.push_back(image);
    int label = std::stoi(dirEntry.path().stem()) / 100;
    y.push_back(label);
  }
  return std::make_tuple(X, y);
}

template<std::size_t TABLE_SIZE = 0>
auto computeGLCMMatrix(const std::vector<cv::Mat>& Xs, const Eigen::MatrixX2i& dirs,
                       std::array<uchar, TABLE_SIZE> quantizeTable = {}) {
  for (cv::Mat X : Xs)
    if (X.depth() != CV_8U)
      throw std::invalid_argument("Image must be 8-bit grayscale");
  int nGrays;
  uchar invQuant[256];
  if constexpr (quantizeTable.empty()) {
    nGrays = 256;
  } else {
    // Get number of gray levels.
    nGrays = 1;
    for (int i = 0; i < 255; i++)
      if (quantizeTable[i] != quantizeTable[i + 1])
        nGrays++;
    // Create inverse lookup table.
    invQuant[0] = 0;
    for (int i = 1; i < 256; i++)
      if (quantizeTable[i - 1] != quantizeTable[i])
        invQuant[i] = invQuant[i - 1] + 1;
      else
        invQuant[i] = invQuant[i - 1];
  }
  std::vector<std::vector<MatrixXd, aligned_allocator<MatrixXd>>> GLCMs;
  GLCMs.reserve(Xs.size());
  // For each image in the dataset...
  for (cv::Mat X : Xs) {
    std::vector<MatrixXd, aligned_allocator<MatrixXd>> imageGLCMs;
    imageGLCMs.reserve(dirs.rows());
    // ...and for each direction...
    for (int k = 0; k < dirs.rows(); k++) {
      // ...compute the GLCM.
      MatrixXi GLCM = MatrixXi::Zero(nGrays, nGrays);
      int di = dirs(k, 0), dj = dirs(k, 1);
      int si = std::abs(std::min(di, 0)), sj = std::abs(std::min(dj, 0));
      int ei = X.rows - std::max(di, 0), ej = X.cols - std::max(dj, 0);
      for (int i = si; i < ei; i++) {
        uchar* Xi = X.ptr<uchar>(i);
        uchar* Xipd = X.ptr<uchar>(i + di);
        for (int j = sj; j < ej; j++) {
          int row = Xi[j], col = Xipd[j + dj];
          if constexpr (quantizeTable.empty())
            GLCM(row, col)++;
          else
            // Compute matrix values using indices from reverse mapping.
            GLCM(invQuant[row], invQuant[col])++;
        }
      }
      imageGLCMs.push_back(GLCM.cast<double>() / ((ei - si) * (ej - sj)));
    }
    GLCMs.push_back(imageGLCMs);
  }
  return GLCMs;
}

MatrixXd
getFeatureVectors(const std::vector<std::vector<MatrixXd,
                  aligned_allocator<MatrixXd>>>& GLCMs) {
  if (GLCMs.empty() || GLCMs[0].empty())
    throw std::invalid_argument("Empty vector of GLCMs");
  std::size_t rows = GLCMs.size(), cols = GLCMs[0].size();
  MatrixXd features(rows, 7 * cols);
  Eigen::ArrayXd arange =
      Eigen::ArrayXd::LinSpaced(GLCMs[0][0].rows(), 0, GLCMs[0][0].rows() - 1);
  Eigen::MatrixXd pairwiseDiff =
    arange.rowwise().replicate(arange.size()).rowwise() - arange.transpose();
  for (std::size_t i = 0; i < rows; i++) {
    for (std::size_t j = 0; j < cols; j++) {
      MatrixXd GLCM = GLCMs[i][j];
      // Maximum probability
      double maxProb = GLCM.maxCoeff();
      // Correlation
      double expectedRow =
          (GLCM.rowwise().sum().array() * arange).sum();
      double expectedColumn =
        (GLCM.colwise().sum().transpose().array() * arange).sum();
      Eigen::ArrayXd deviationsRow = arange - expectedRow;
      Eigen::ArrayXd deviationsColumn = arange - expectedColumn;
      double varianceRow =
        (GLCM.rowwise().sum().array() * deviationsRow.square()).sum();
      double varianceColumn =
        (GLCM.colwise().sum().transpose().array() * deviationsColumn.square()).sum();
      double correlation =
        (deviationsRow.matrix().transpose() * (GLCM * deviationsColumn.matrix()))[0] /
        std::sqrt(varianceRow * varianceColumn);
      // Contrast
      double contrast = (pairwiseDiff.array().square() * GLCM.array()).sum();
      // Uniformity
      double uniformity = GLCM.array().square().sum();
      // Homogeneity
      double homogeneity = (GLCM.array() / (1 + pairwiseDiff.array().abs())).sum();
      // Entropy
      Eigen::MatrixXd zeroCorrectedGLCM = (GLCM.array() == 0)
        .select(MatrixXd::Ones(GLCM.rows(), GLCM.cols()), GLCM);
      double entropy = -(GLCM.array() * zeroCorrectedGLCM.array().log2()).sum();
      // Dissimilarity
      double dissimilarity = (GLCM.array() * pairwiseDiff.array().abs()).sum();

      features.block<1, 7>(i, 7 * j) << maxProb, correlation, contrast,
                                        uniformity, homogeneity, entropy,
                                        dissimilarity;
    }
  }
  return features;
}

template<int QUANTIZE = 256>
MatrixXd extractFeatures(const std::vector<cv::Mat>& Xs, int dirRange) {
  // Compute directions for GLCMs.
  // int r = 0;
  // int dirRows = std::pow(2 * dirRange + 1, 2) - 1;
  // Eigen::MatrixX2i dirs(dirRows, 2);
  // for (int i = -dirRange; i <= dirRange; i++)
  //   for (int j = -dirRange; j <= dirRange; j++)
  //     dirs(r, 0) = i, dirs(r++, 1) = j;
  // Downscale X to 'QUANTIZE' levels.
  Eigen::MatrixX2i dirs(4, 2);
  dirs << 1,  1,
          1, -1,
         -1,  1,
         -1, -1;
  std::vector<cv::Mat> quantizedXs;
  quantizedXs.reserve(Xs.size());
  constexpr std::array<uchar, 256> lookupTable = [] {
    std::array<uchar, 256> table = {};
    for (int i = 0; i < 256; i++)
      table[i] = QUANTIZE * (i / QUANTIZE);
    return table;
  }();
  for (std::size_t i = 0; i < Xs.size(); i++) {
    cv::Mat quantizedX;
    cv::LUT(Xs[i], lookupTable, quantizedX);
    quantizedXs.push_back(quantizedX);
  }
  // Compute GLCM.
  std::vector<std::vector<MatrixXd, aligned_allocator<MatrixXd>>> GLCMs;
  if constexpr (QUANTIZE < 256)
    GLCMs = computeGLCMMatrix(quantizedXs, dirs, lookupTable);
  else
    GLCMs = computeGLCMMatrix(Xs, dirs);
  return getFeatureVectors(GLCMs);
}

template<bool returnPermutations = false>
auto splitTrainTest(const MatrixXd& Xs, std::vector<int> ys,
                    int nClass, int sizeClass, int sizeTrain) {
  // Wrap std::vector data in Eigen object.
  Eigen::VectorXi ysEigen = Eigen::Map<Eigen::VectorXi,
                                       Eigen::Unaligned>(ys.data(), ys.size());
  int sizeTest = sizeClass - sizeTrain;
  MatrixXd trainX(nClass * sizeTrain, Xs.cols());
  Eigen::VectorXi trainY(nClass * sizeTrain);
  MatrixXd testX(nClass * sizeTest, Xs.cols());
  Eigen::VectorXi testY(nClass * sizeTest);
  std::vector<Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic>> permutations;
  if constexpr (returnPermutations)
    permutations.reserve(nClass);
  std::random_device rd;
  std::mt19937 gen(rd());
  Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> perm(sizeClass);
  perm.setIdentity();
  for (int i = 0; i < nClass; i++) {
    std::shuffle(perm.indices().data(),
                 perm.indices().data() + sizeClass,
                 gen);
    if constexpr (returnPermutations)
      permutations.push_back(perm);
    MatrixXd shuffledX = perm * Xs.block(i * sizeClass, 0, sizeClass, Xs.cols());
    MatrixXd trainBlock = shuffledX.block(0, 0, sizeTrain, Xs.cols());
    MatrixXd testBlock = shuffledX.block(sizeTrain, 0, sizeTest, Xs.cols());
    trainX.block(i * sizeTrain, 0,
                 sizeTrain, Xs.cols()) = trainBlock;
    testX.block(i * sizeTest, 0,
                sizeTest, Xs.cols()) = testBlock;
    // Shuffle labels along with data.
    ysEigen.segment(i * sizeClass, sizeClass) =
        perm * ysEigen.segment(i * sizeClass, sizeClass);
    trainY.segment(i * sizeTrain, sizeTrain) =
      ysEigen.segment(i * sizeClass, sizeTrain);
    testY.segment(i * sizeTest, sizeTest) =
      ysEigen.segment(sizeTrain + i * sizeClass, sizeTest);
  }
  if constexpr (returnPermutations)
    return std::make_tuple(trainX, trainY, testX, testY, permutations);
  else
    return std::make_tuple(trainX, trainY, testX, testY);
}

template<int P, bool SELECT = false>
RowMatrixXi findClosest(const MatrixXd& train, const MatrixXd& queries, int k = 5) {
  // Compute distance matrix.
  RowMatrixXi closest(queries.rows(), k);
  RowMatrixXd distances(queries.rows(), train.rows());
  for (int i = 0; i < queries.rows(); i++) {
    distances.row(i) =
      (train.rowwise() - queries.row(i)).rowwise().lpNorm<P>();
  }
  std::vector<int> idxs(distances.cols());
  int i = 0;
  for (const auto& row : distances.rowwise()) {
    auto comparator = [&row](const int &a, const int &b) {
      return row(a) < row(b);
    };
    // Generate indices.
    std::iota(idxs.begin(), idxs.end(), 0);
    if constexpr (SELECT)
      // Select first k indices using distance as comparison key.
      std::nth_element(idxs.begin(), idxs.begin() + (k - 1), idxs.end(),
                       comparator);
    else
      // Sort all indices using distance as comparison key.
      std::sort(idxs.begin(), idxs.end(), comparator);
    std::copy(idxs.begin(), idxs.begin() + k, closest.row(i++).begin());
  }
  return closest;
}

void logPerformanceMetrics(const MatrixXi& retrievedLabels,
                           const Eigen::VectorXi& testLabels,
                           int numClasses = 10) {
  Eigen::Index retrievedCols = retrievedLabels.cols();
  Eigen::Index retrievedRows = retrievedLabels.rows();
  std::initializer_list<Eigen::Index> nRetrieved =
    {10, 30, 50, 100, retrievedCols};
  for (Eigen::Index nret : nRetrieved)
    if (nret < retrievedCols) {
      MatrixXi currentRetrievedLabels =
        retrievedLabels.block(0, 0, retrievedRows, nret);
      MatrixXi currentTestLabels =
          testLabels.rowwise().replicate(nret);
      MatrixXi confusionMatrix = MatrixXi::Zero(numClasses, numClasses);
      for (int i = 0; i < currentTestLabels.rows(); i++)
        for (int j = 0; j < currentTestLabels.cols(); j++) {
          int row = currentRetrievedLabels(i, j);
          int col = currentTestLabels(i, j);
          confusionMatrix(row, col)++;
        }
      Eigen::VectorXd diag = confusionMatrix.diagonal().cast<double>();
      Eigen::VectorXd sumCWise =
        confusionMatrix.colwise().sum().cast<double>();
      Eigen::VectorXd sumRWise =
        confusionMatrix.rowwise().sum().cast<double>();
      Eigen::VectorXd recall = diag.array() / sumCWise.array();
      Eigen::VectorXd precision = diag.array() / sumRWise.array();
      std::cout << "------------------------------\n";
      std::cout << "Number retrieved per query: " << nret << " \n";
      for (int i = 0; i < numClasses; i++) {
        std::cout << "Class: " << i << '\n';
        std::cout << "\tPrecision (" << nret <<"): " << precision(i) << '\n';
        std::cout << "\tRecall (" << nret << "): " << recall(i) << '\n';
      }
      std::cout << "Number of retrieved elements: " << confusionMatrix.sum()
                << '\n';
      std::cout << "Confusion matrix:\n";
      std::cout << confusionMatrix << '\n';
      std::cout << "Average precision (" << nret << "): "
                << precision.mean() << '\n';
      std::cout << "Average recall (" << nret << "): "
                << recall.mean() << '\n';
    }
}

int main() {
  using ms = std::chrono::milliseconds;
  std::chrono::steady_clock::time_point timeStart;
  std::chrono::steady_clock::time_point timeEnd;
  int k = 100;
  std::cout << "Reading data...\n";
  timeStart = std::chrono::steady_clock::now();
  auto [X, y] = readData("data/lab4");
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  std::cout << "Constructing feature vectors...\n";
  timeStart = std::chrono::steady_clock::now();
  MatrixXd featureMatrix = extractFeatures<256>(X, 3);
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  std::cout << "Splitting into train and test sets...\n";
  timeStart = std::chrono::steady_clock::now();
  auto [trainX, trainY, testX, testY, permutations] =
    splitTrainTest<true>(featureMatrix, y, 10, 100, 70);
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  std::cout << "Finding closest " << k << " points...\n";
  timeStart = std::chrono::steady_clock::now();
  MatrixXi closestIndices = findClosest<1, false>(trainX, testX, k);
  timeEnd = std::chrono::steady_clock::now();
  std::cout << "Time difference = "
            << std::chrono::duration_cast<ms>(timeEnd - timeStart).count()
            << " [ms]\n";
  auto labelFetchLambda = [&trainY](int elem) { return trainY(elem); };
  MatrixXi retrievedY = closestIndices.unaryExpr(labelFetchLambda);
  // for (int i = 0; i < retrievedY.rows(); i++)
  //   std::cout << retrievedY(i, Eigen::all) << " -> " << testY[i] <<
  //   std::endl;
  logPerformanceMetrics(retrievedY, testY, 10);
  // Save some queries along with their top 5 most similar responses.
  // First, permute the images using the random permutations returned
  // by the splitTrainTest function.
  std::vector<int> idxs(X.size());
  std::iota(idxs.begin(), idxs.end(), 0);
  Eigen::VectorXi idxsEigen =
    Eigen::Map<Eigen::VectorXi, Eigen::Unaligned>(idxs.data(), idxs.size());
  for (int i = 0; i < 10; i++) {
    idxsEigen.segment<100>(i * 100) =
      permutations[i].transpose() * idxsEigen.segment<100>(i*100);
  }
  for (int i = 0; i < 3; i++) {
    int queryIdx = idxsEigen(70 + i);
    cv::Mat queryImage = X[queryIdx];
    std::ostringstream ss1;
    ss1 << "output/lab4/images/query" << i << ".jpg";
    std::string outputPathString1 = ss1.str();
    cv::imwrite(outputPathString1, queryImage);
    for (int k = 0; k < 5; k++) {
      int retrievedIdx = idxsEigen(closestIndices(i, k));
      cv::Mat retrievedImage = X[retrievedIdx];
      std::ostringstream ss2;
      ss2 << "output/lab4/images/retrieved" << i << "_" << k << ".jpg";
      std::string outputPathString2 = ss2.str();
      cv::imwrite(outputPathString2, retrievedImage);
    }
  }
  return EXIT_SUCCESS;
}


#ifndef OBVCLIP_POLYTOPE_HPP
#define OBVCLIP_POLYTOPE_HPP

#include <Eigen/Dense>
#include <vector>
#include <limits>
#include <iterator>
#include <memory>
#include <iostream>

#include <oblog.hpp>
#include <obconfig.hpp>

//forward declarations
namespace orgQhull {class Qhull; class QhullFacet;}

namespace OB
{

    class ConvexPolytope;

    constexpr FeatureId InvalidFeatureId = (std::numeric_limits<FeatureId>::max)();

    enum class FeatureType
        {
         vertex, edge, face, empty
        };

    struct Feature
    {
        Feature() {}
        Feature(FeatureId fid, FeatureType t)
            :feature(fid), type(t) {}
        FeatureId feature = -1;
        FeatureType type = FeatureType::empty;

        bool isVertex() const { return type == FeatureType::vertex; }
        bool isEdge() const { return type == FeatureType::edge; }
        bool isFace() const { return type == FeatureType::face; }

        friend bool operator<(const Feature& l, const Feature& r)
        {
            return std::tie(l.type, l.feature) < std::tie(r.type, r.feature);
        }
        friend bool operator<=(const Feature& l, const Feature& r)
        {
            return std::tie(l.type, l.feature) <= std::tie(r.type, r.feature);
        }

        friend bool operator==(const Feature& l, const Feature& r)
        {
            return (l.type == r.type && l.feature == r.feature);
        }
        friend bool operator!=(const Feature& l, const Feature& r)
        {
            return !(l==r);
        }

        friend std::ostream& operator<< (std::ostream &out, const Feature &f);
    };

    struct EdgePoints
    {
        EdgePoints() = default;
        EdgePoints(const Point& t, const Point& h)
            :tail(t), head(h)
        {}
        Point tail = Point::Zero();
        Point head = Point::Zero();
        friend std::ostream& operator<< (std::ostream &out, const EdgePoints &f)
        {
            out << "{ tail: " << f.tail.transpose() << " head: " << f.head.transpose() << "}";
            return out;
        }
        Point vector() const
        {
            return head - tail;
        }
        Point midpoint() const
        {
            return 0.5 * (head + tail);
        }

        Point point_at_lambda(Real lambda) const {
            return (1-lambda) * tail + lambda * head;
        }
    };

    std::ostream& operator<< (std::ostream &out, const Plane& p);
    struct VoronoiPlane
    {
        VoronoiPlane(const Plane& p, Feature f): plane(p), feature(f) {}

        Plane plane;
        Feature feature; // feature on the other side of the plane
        friend std::ostream& operator<< (std::ostream &out, const VoronoiPlane &vp) {
            out << " {VoronoiPlane plane: " << vp.plane << " feature: " << vp.feature << " }";
            return out;
        }
    };

    struct FacePerimeterFeature {
        FacePerimeterFeature(const Feature& f,
                             const std::array<VoronoiPlane, 2>& ps)
            : feature(f), vertexedge_vps(ps) {}

        FacePerimeterFeature(const Feature& f,
                             const std::array<VoronoiPlane, 2>& ps,
                             const Plane& fep)
            : feature(f), vertexedge_vps(ps), faceedge_plane(fep)  {}

        Feature feature; // vertex or edge
        std::array<VoronoiPlane, 2> vertexedge_vps; //voronoi planes (edge-vertex)
        Plane faceedge_plane{Vector::Zero(),0}; // ONLY FOR EDGES GRRRR
    };

    // some simple helper functions.
    Real dist_point_point(const Point& point1, const Point& point2);
    Real dist_point_line(const Point& point1, const EdgePoints& edge);
    // absolute distance
    Real absdist_point_plane(const Point& point, const Plane& plane);
    Real dist_line_line(const EdgePoints& edge1, const EdgePoints& edge2);
    Real dist_point_edge(const Point& point, const EdgePoints& edge);
    Real dist_edge_edge(const EdgePoints& edge1, const EdgePoints& edge2);
    // convex face, determined by list of edges in CCW order.
    // returns an absolut distance. Very slow.
    Real absdist_point_face(const Point& point,
                           const Plane& facePlane,
                           const std::vector<EdgePoints>& faceEdgesCcw);
    // very slow.
    Real absdist_edge_face(const EdgePoints& edgePoints,
                          const Plane& facePlane,
                          const std::vector<EdgePoints>& faceEdgesCcw);

    class WorldConvexPolytope;
    Real absdist_between_features(const WorldConvexPolytope& wcp1, const Feature& f1,
                                  const WorldConvexPolytope& wcp2, const Feature& f2);

    // return true if there is an intersection point.
    std::pair<bool, Point> intersect_line_plane(const EdgePoints& edgePoints,
                                                const Plane& facePlane);

    //edge is a line. returns a point in that line, not clipped
    Point closest_point_to_line(const Point& point, const EdgePoints& edge);
    std::pair<Point, Point> closests_line_to_line(const EdgePoints& e1, const EdgePoints& e2);

    class WorldConvexPolytope
    {
    public:
        WorldConvexPolytope(const WorldConvexPolytope&) = delete;
        WorldConvexPolytope& operator=(const WorldConvexPolytope&) = delete;

        WorldConvexPolytope(const std::string& name, std::shared_ptr<ConvexPolytope> cv)
            :name(name), polytope(std::move(cv)) {}

        WorldConvexPolytope(const std::string& name,
                            std::shared_ptr<ConvexPolytope> cv,
                            const Transform& p)
            : name(name), polytope(std::move(cv)), pose(p) {}

        const ConvexPolytope& convex_polytope() const { return *polytope; }

        void set_pose (const Transform& newPose) { pose = newPose; }
        void set_pose (const Translation& translation, const Quaternion& q)
        { pose = translation * q; }

        const Transform& get_pose() const { return pose; }
        const std::string& get_name() const { return name; }

        friend std::ostream& operator<< (std::ostream &out, const WorldConvexPolytope &wcp);

        // there is only one object equal to this one. This one.
        //This weird thing is for vclip cache comparison
        bool operator == ( const WorldConvexPolytope& other) const
        { return this == std::addressof(other); }
        bool operator != (const WorldConvexPolytope &other) const
        { return this != std::addressof(other); }
        //Arbitrary order, this is for the cache.
        bool operator> (const WorldConvexPolytope &other) const
        { return this > std::addressof(other); }
        bool operator<= (const WorldConvexPolytope &other) const
        { return this <= std::addressof(other); }
        bool operator< (const WorldConvexPolytope &other) const
        { return this < std::addressof(other); }
        bool operator>= (const WorldConvexPolytope &other) const
        { return this >= std::addressof(other); }

    private:
        const std::string name;
        const std::shared_ptr<ConvexPolytope> polytope;
        Transform pose = Transform::Identity();
    };
}

namespace std
{
    template<> struct hash<::OB::WorldConvexPolytope> {
        size_t operator()(const ::OB::WorldConvexPolytope& val) const{
            return std::hash<const ::OB::WorldConvexPolytope*>{}(std::addressof(val));
        }
    };
}

namespace OB
{
    class Vertex
    {
    public:
        Vertex() = default;
        Vertex(const Point& pos):position{pos} {}
        bool has_halfedge() const { return halfedge != InvalidFeatureId; }
        FeatureId get_halfedge_id() const {return halfedge; }
        void set_halfedge_id(FeatureId new_halfedge_id)  { halfedge = new_halfedge_id; }

        const Point& get_position() const { return position; }
    private:
        Point position;
        // TODO we could put this field in the fourth position of vector3d.
        // Outgoing half edge (this vertex is the tail vertex of the half edge)
        FeatureId halfedge{InvalidFeatureId};
        friend std::ostream& operator<< (std::ostream &out, const Vertex &vertex);
    };
    using VertexList = std::vector<Vertex>;

    class HalfEdge
    {
    public:
        //Pair half edge
        void set_pair_id(FeatureId new_pair_id) { pair = new_pair_id; }
        FeatureId get_pair_id() const { return pair; }

        void set_next_id(FeatureId new_next) { next_halfedge_id = new_next; }
        FeatureId get_next_id() const { return next_halfedge_id; }

        void set_headvertex_id(FeatureId new_headvertex_id) { headvertex_id = new_headvertex_id; }
        FeatureId get_headvertex_id() const { return headvertex_id; }

        void set_face_id(FeatureId new_face_id) { face_id = new_face_id; }
        FeatureId get_face_id() const { return face_id; }

        const Plane& get_faceedge_plane() const { return faceedge_plane; }
        void set_faceedge_plane(const Plane& p) { faceedge_plane = p; }

        const Plane& get_headvertexedge_plane() const  { return headvertexedge_plane; }
        void set_headvertexedge_plane(const Plane& p) { headvertexedge_plane = p; }

    private:
        //we could have this implicit, having the list
        //of halfedges with the pairs in consecutive positions.
        //for now do it this way.
        FeatureId pair;
        // next in face, counter clock wise
        FeatureId next_halfedge_id;
        FeatureId headvertex_id;
        FeatureId face_id;

        // the hyperplanes normals point to the Voronoi Region
        // of the Edge. Must be flipped when necessary.
        Plane faceedge_plane;
        Plane headvertexedge_plane; // plane vertex-edge for the head vertex
        friend std::ostream& operator<< (std::ostream &out, const HalfEdge &halfEdge);
    };
    using HalfEdgeList = std::vector<HalfEdge>;

    class Face
    {
    public:
        Face(const Plane& plane) : plane(plane) {}
        void set_halfedge_id(FeatureId new_halfedge_id) { halfedge_id = new_halfedge_id; }
        FeatureId get_halfedge_id() const { return halfedge_id; }

        const Plane& get_plane() const { return plane; }

        //this functions are slow, for debugging.
        Point get_center(const ConvexPolytope& cv) const;
    private:
        FeatureId halfedge_id{InvalidFeatureId};
        Plane plane;
        friend std::ostream& operator<< (std::ostream &out, const Face &face);
    };
    using FaceList = std::vector<Face>;

    class ConvexPolytope
    {
    public:
        ConvexPolytope():generated{false} {}
        ConvexPolytope(const ConvexPolytope& o) = default;
        void generate_from_vertices(const std::vector<Point>& points);
        bool euler_number_correct();

        //This are the functions used in vclip, that are do not
        //depend on the half edge structure (could be winged edge or other)

        //get voronoi planes vertex edge VP(V,E) for a vertex
        std::vector<VoronoiPlane> get_vpve_vertex(Feature vid) const {
            return std::vector<VoronoiPlane>(begin_vpve_vertex(vid), end_vpve_vertex(vid));
        }

        //VP(V,E), voronoi plane vertex edge (vpve).
        class it_vpve_vertex;
        it_vpve_vertex begin_vpve_vertex(Feature vertex_id) const {
            return it_vpve_vertex(*this, vertex_id, true);
        }
        it_vpve_vertex end_vpve_vertex(Feature vertex_id) const  {
            return it_vpve_vertex(*this, vertex_id, false);
        }

        //VP(V,E)
        std::array<VoronoiPlane, 2> get_vpve_edge(Feature hid) const;

        //VP(F,E)
        std::array<VoronoiPlane, 2> get_vpfe_edge(Feature hid) const;

         //VP(F,E)
        std::vector<VoronoiPlane> get_vpfe_face(Feature fid) const {
            return std::vector<VoronoiPlane>(begin_vpfe_face(fid),
                end_vpfe_face(fid));
        }
        //VP(F,E)
        class it_vpfe_face;
        it_vpfe_face begin_vpfe_face(Feature fid) const {
            return it_vpfe_face(*this, fid, true);
        }
        it_vpfe_face end_vpfe_face(Feature fid) const {
            return it_vpfe_face(*this, fid, false);
        }

        Plane get_face_plane(Feature fid) const {
            assert (fid.type == FeatureType::face);
            return get_face(fid.feature).get_plane();
        }

        // first = tail, second = head
        EdgePoints get_edgepoints(Feature heid) const {
            assert (heid.type == FeatureType::edge);
            const HalfEdge &he = get_halfedge(heid.feature);
            const Point& head = get_vertex(he.get_headvertex_id()).get_position();
            const Point& tail =
                get_vertex(get_halfedge(he.get_pair_id()).get_headvertex_id()).get_position();
            return EdgePoints{tail, head};
        }

        // edges (outgoind half edge) from a vertex with the other point in edge
        std::vector<std::pair<Feature, Point>> get_edgepoints_for_vertex(Feature vid) const {
            return  std::vector<std::pair<Feature, Point>>(begin_edgepoints_vertex(vid),
                end_edgepoints_vertex(vid));
        }

        // edges (outgoind half edge) from a vertex with the other point in edge
        class it_ep_vertex;
        it_ep_vertex begin_edgepoints_vertex(Feature vid) const {
            return it_ep_vertex(*this, vid, true);
        }
        it_ep_vertex end_edgepoints_vertex(Feature vid) const {
            return it_ep_vertex(*this, vid, false);
        }

        Point get_vertex_point(Feature v) const {
            assert (v.type == FeatureType::vertex);
            return get_vertex(v.feature).get_position();
        }

        std::vector<Point> get_face_points(Feature fid) const;
        std::vector<Feature> get_face_vertices(Feature fid) const;
        std::vector<EdgePoints> get_face_edgepoints(Feature fid) const;

        // slow and too much memory allocated, rethink this
        std::vector<std::pair<Feature, Plane>> get_faces_with_planes() const {
            std::vector<std::pair<Feature, Plane>> resp;
            for (auto it = faces.begin(); it != faces.end(); ++it)
            {
                FeatureId fid = std::distance(faces.begin(), it);
                resp.emplace_back(Feature{fid, FeatureType::face}, it->get_plane());
            }
            L_(linfo) << "get_faces_with_planes " << resp.size();
            return resp;
        };

        class it_planes_faces;
        it_planes_faces begin_planes_faces() const { return it_planes_faces(faces, faces.begin()); }
        it_planes_faces end_planes_faces() const { return it_planes_faces(faces, faces.end()); }

        // For face voronoi region exclusion. Mirtich 3.3.3
        // the VoronoiPlane info is the opposite of the normal way.
        // the feature it includes is the current one, not the other side one
        std::vector<FacePerimeterFeature> get_scan_face(Feature fid) const {
            return  std::vector<FacePerimeterFeature >(begin_scan_face(fid),
                end_scan_face(fid));
        }

        class it_scanning_face;
        it_scanning_face begin_scan_face(Feature fid) const {
            return it_scanning_face(*this, fid, true);
        }
        it_scanning_face end_scan_face(Feature fid) const {

            return it_scanning_face(*this, fid, false);
        }

        Feature get_edge_headvertex(Feature fe) const {
            assert (fe.type == FeatureType::edge);
            return Feature{get_halfedge(fe.feature).get_headvertex_id(), FeatureType::vertex };
        }

        Feature get_edge_tailvertex(Feature fe) const {
            assert (fe.type == FeatureType::edge);
            return Feature{get_halfedge(get_halfedge(fe.feature).get_pair_id()).get_headvertex_id(),
                    FeatureType::vertex };
        }

        // maybe private?
        const Vertex& get_vertex(FeatureId vId) const{
            return vertices[vId];
        }
        const HalfEdge& get_halfedge(FeatureId heId) const{
            return halfedges[heId];
        }
        HalfEdge& get_halfedge(FeatureId heId){
            return halfedges[heId];
        }
        const Face& get_face(FeatureId faceId) const{
            return faces[faceId];
        }
        Face& get_face(FeatureId faceId) {
            return faces[faceId];
        }
        FaceList& get_faces() { return faces; }
        const FaceList& get_faces() const { return faces; }
        VertexList& get_vertices() { return vertices; }
        const VertexList& get_vertices() const { return vertices; }
        HalfEdgeList& get_halfedges() { return halfedges; }

        void get_local_aabb(Vector& min_point, Vector &max_point) const;

        // this is just for testing
        void write(std::ostream& ost);
        static ConvexPolytope create(std::istream& ifs);

        // IDEAS ON ITERATORS:
        // https://blog.cppse.nl/cpp-multiple-iterators-with-traits
        // QUITAR EL std::iterator, que está deprecado!
        class it_vpve_vertex : public std::iterator<std::input_iterator_tag, VoronoiPlane>
        {
        public:
            explicit it_vpve_vertex(const ConvexPolytope& polytope, Feature vertex_id, bool first)
                :polytope(polytope), halfedge_id(polytope.get_vertex(vertex_id.feature).get_halfedge_id()), first(first)
            {
                assert(vertex_id.type == FeatureType::vertex);
            };
            value_type operator*() const
            {
                FeatureId nextPairId = polytope.get_halfedge(halfedge_id).get_pair_id();
                const HalfEdge& incomingHalfEdge = polytope.get_halfedge(nextPairId);
                VoronoiPlane vp =  VoronoiPlane(incomingHalfEdge.get_headvertexedge_plane(),
                                                Feature{nextPairId, FeatureType::edge});
                vp.plane.coeffs()*=-1;
                return vp;

            }
            it_vpve_vertex operator++() {
                it_vpve_vertex i = *this;
                first = false;
                advance();
                return i;
            }
            it_vpve_vertex operator++(int junk) {
                first = false;
                advance();
                return *this;
            }
            bool operator!=(const it_vpve_vertex &o) const
            {
                return halfedge_id != o.halfedge_id || first != o.first;
            }
        private:
            inline void advance() {
                halfedge_id = polytope.get_halfedge(halfedge_id).get_pair_id();
                halfedge_id = polytope.get_halfedge(halfedge_id).get_next_id();
            }
            const ConvexPolytope& polytope;
            FeatureId halfedge_id;
            bool first;
        };

        class it_vpfe_face : public std::iterator<std::input_iterator_tag, VoronoiPlane>
        {
        public:
            explicit it_vpfe_face(const ConvexPolytope& polytope, Feature fid, bool first)
                :polytope(polytope), halfedge_id(polytope.get_face(fid.feature).get_halfedge_id())
                ,first(first) {
                assert(fid.type == FeatureType::face);
            }
            value_type operator*() const
            {
                const HalfEdge &he = polytope.get_halfedge(halfedge_id);
                VoronoiPlane vp =  VoronoiPlane(he.get_faceedge_plane(),
                                                Feature{halfedge_id, FeatureType::edge});
                vp.plane.coeffs()*=-1;
                return vp;
            }
            it_vpfe_face operator++() {
                it_vpfe_face i = *this;
                advance();
                return i;
            }
            it_vpfe_face operator++(int junk) {
                advance();
                return *this;
            }
            bool operator!=(const it_vpfe_face &o) const
            {
                return halfedge_id != o.halfedge_id || first != o.first;
            }
        private:
            inline void advance() {
                first = false;
                halfedge_id = polytope.get_halfedge(halfedge_id).get_next_id();
            }
            const ConvexPolytope& polytope;
            FeatureId halfedge_id;
            bool first;
        };

        //ep is edge point. this iterates over all the points connected by an edge to that vertex,
        //except the vertex original point.
        class it_ep_vertex : public std::iterator<std::input_iterator_tag, std::pair<Feature, Point>>
        {
        public:
            explicit it_ep_vertex(const ConvexPolytope& polytope, Feature vertex_id, bool first)
                :polytope(polytope),
                halfedge_id(polytope.get_vertex(vertex_id.feature).get_halfedge_id()),
                first(first) {
                assert(vertex_id.type == FeatureType::vertex);
            }
            value_type operator*() const
            {
                const HalfEdge& outgoingHalfEdge = polytope.get_halfedge(halfedge_id);
                Point otherPoint = polytope.get_vertex(outgoingHalfEdge.get_headvertex_id()).get_position();
                return std::make_pair(Feature{halfedge_id, FeatureType::edge}, otherPoint);
            }
            it_ep_vertex operator++() {
                it_ep_vertex i = *this;
                advance();
                return i;
            }
            it_ep_vertex operator++(int junk) {
                advance();
                return *this;
            }
            bool operator!=(const it_ep_vertex &o) const
            {
                return halfedge_id != o.halfedge_id || first != o.first;
            }
        private:
            inline void advance() {
                first = false;
                const HalfEdge& outgoingHalfEdge = polytope.get_halfedge(halfedge_id);
                halfedge_id = outgoingHalfEdge.get_pair_id(); //ingoing
                halfedge_id = polytope.get_halfedge(halfedge_id).get_next_id(); //outgoing
            }
            const ConvexPolytope& polytope;
            FeatureId halfedge_id;
            bool first;
        };

        class it_scanning_face : public std::iterator<std::input_iterator_tag, FacePerimeterFeature>
        {
        public:
            explicit it_scanning_face(const ConvexPolytope& polytope,
                                      Feature face_id, bool first)
                :polytope(polytope),
                halfedge_id(polytope.get_face(face_id.feature).get_halfedge_id()), first(first) {
                assert(face_id.type == FeatureType::face);
            }

            value_type operator*() const
            {
                const HalfEdge &he = polytope.get_halfedge(halfedge_id);
                const HalfEdge &pairhe = polytope.get_halfedge(he.get_pair_id());
                Feature fedge{halfedge_id, FeatureType::edge};
                Feature fvertex{he.get_headvertex_id(), FeatureType::vertex };
                if (in_edge)
                {
                    const Vertex &vertex = polytope.get_vertex(fvertex.feature);
                    Feature bvertex{pairhe.get_headvertex_id(), FeatureType::vertex };
                    Plane backPlane = pairhe.get_headvertexedge_plane();
                    Plane frontPlane = he.get_headvertexedge_plane();
                    Plane faceEdgePlane = he.get_faceedge_plane();
                    faceEdgePlane.coeffs()*=-1;
                    return FacePerimeterFeature{
                        fedge, std::array<VoronoiPlane, 2>{VoronoiPlane(backPlane, bvertex),
                                VoronoiPlane(frontPlane, fvertex)}, faceEdgePlane
                    };
                }
                else
                {
                    Feature fNextEdge{he.get_next_id(), FeatureType::edge};
                    const HalfEdge &nextPairHe = polytope.get_halfedge(polytope.get_halfedge(he.get_next_id()).get_pair_id());
                    Plane backNextEdgePlane = nextPairHe.get_headvertexedge_plane();
                    Plane backPlane = pairhe.get_headvertexedge_plane();
                    Plane frontPlane = he.get_headvertexedge_plane();
                    frontPlane.coeffs()*=-1;
                    backNextEdgePlane.coeffs()*=-1;
                    return FacePerimeterFeature{
                        fvertex,
                            std::array<VoronoiPlane, 2>{VoronoiPlane(frontPlane, fedge),
                                VoronoiPlane(backNextEdgePlane, fNextEdge)}
                    };
                }
            }
            it_scanning_face operator++() {
                it_scanning_face i = *this;
                advance();
                return i;
            }
            it_scanning_face operator++(int junk) {
                advance();
                return *this;
            }
            bool operator!=(const it_scanning_face &o) const
            {
                return (halfedge_id != o.halfedge_id ||
                        first != o.first || in_edge != o.in_edge);
            }

        private:
            inline void advance() {
                first = false;
                if (!in_edge) {
                    const HalfEdge &he = polytope.get_halfedge(halfedge_id);
                    halfedge_id = he.get_next_id();
                }
                in_edge = !in_edge;
            }
            const ConvexPolytope& polytope;
            bool in_edge = true;
            FeatureId halfedge_id;
            bool first;
        };

        class it_planes_faces : public std::iterator<std::input_iterator_tag,
                                                     std::pair<Feature, Plane>>
        {
        public:
            explicit it_planes_faces(const FaceList& list, FaceList::const_iterator it)
                :list(list), it(it) {}

            value_type operator*() const
            {
                FeatureId fid = std::distance(list.begin(), it);
                return std::make_pair(Feature{fid, FeatureType::face}, it->get_plane());
            }
            it_planes_faces operator++() {
                it_planes_faces i = *this;
                ++it;
                return i;
            }
            it_planes_faces operator++(int junk) {
                ++it;
                return *this;
            }
            bool operator!=(const it_planes_faces &o) const
            {
                return it!=o.it;
            }

        private:
            const FaceList& list;
            FaceList::const_iterator it;
        };

    private:
        bool generated;
        VertexList vertices;
        HalfEdgeList halfedges;
        FaceList faces;
        //TODO MAYBE CENTER/ORIGIN/VOLUME OR THE CONVEX POLYTOPE??

        void create_vertices(const orgQhull::Qhull&);
        void create_face_and_halfedges(orgQhull::QhullFacet&);
        void pair_halfedges();
        void generate_voronoi_planes();

        friend std::ostream& operator<< (std::ostream &out, const ConvexPolytope &point);
    };

    std::ostream& operator<< (std::ostream &out, const Vertex &vertex);
    std::ostream& operator<< (std::ostream &out, const Face &face);
    std::ostream& operator<< (std::ostream &out, const HalfEdge &halfEdge);
    std::ostream& operator<< (std::ostream &out, const ConvexPolytope &cp);
    std::ostream& operator<< (std::ostream &out, const WorldConvexPolytope &wcp);
    std::ostream& operator<< (std::ostream &out, const Feature& f);
    std::ostream& operator<< (std::ostream &out, const FeatureType& ft);

}

#endif


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (C) 2019-2021 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */
#include "opendrive/geometry/LaneUtils.hpp"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <spdlog/spdlog.h>
#include <vector>
#include "opendrive/geometry/Geometry.hpp"

namespace opendrive {
namespace geometry {

typedef ::boost::geometry::model::d2::point_xy<double> Boost2dPoint;
typedef ::boost::geometry::model::polygon<Boost2dPoint> BoostPolygon;

/**
 * Convert lane to boost polygon
 *
 * move the lane edges by the given margin to the center (shrink the lane)
 */
BoostPolygon fromLane(Lane const &lane, double const margin)
{
  auto const &leftBorder = lane.leftEdge;
  auto const &rightBorder = lane.rightEdge;

  std::vector<Boost2dPoint> leftPoints;
  std::vector<Boost2dPoint> rightPoints;

  for (auto it = leftBorder.begin(); it != leftBorder.end(); ++it)
  {
    Boost2dPoint point({it->x, it->y});
    // leave out consecutive identical points
    if (leftPoints.empty() || (boost::geometry::distance(leftPoints.back(), point) > 0.001))
    {
      leftPoints.push_back(point);
    }
  }
  for (auto it = rightBorder.begin(); it != rightBorder.end(); ++it)
  {
    Boost2dPoint point({it->x, it->y});
    // leave out consecutive identical points
    if (rightPoints.empty() || (boost::geometry::distance(rightPoints.back(), point) > 0.001))
    {
      rightPoints.push_back(point);
    }
  }

  Boost2dPoint orthogonalDirection;
  size_t i = 0;
  if (leftPoints.size() > 1)
  {
    for (; i < leftPoints.size() - 1; i++)
    {
      Boost2dPoint direction = leftPoints[i + 1];
      boost::geometry::subtract_point(direction, leftPoints[i]);
      double normalizingFactor = 1.0 / boost::geometry::distance(leftPoints[i + 1], leftPoints[i]);
      Boost2dPoint normalizedDirection = direction;
      boost::geometry::multiply_value(normalizedDirection, normalizingFactor);

      orthogonalDirection.x(normalizedDirection.y());
      orthogonalDirection.y(-normalizedDirection.x());
      boost::geometry::multiply_value(orthogonalDirection, margin);
      boost::geometry::add_point(leftPoints[i], orthogonalDirection);
    }
    boost::geometry::add_point(leftPoints[i], orthogonalDirection);
  }

  if (rightPoints.size() > 1)
  {
    i = 0;
    for (; i < rightPoints.size() - 1; i++)
    {
      Boost2dPoint direction = rightPoints[i + 1];
      boost::geometry::subtract_point(direction, rightPoints[i]);
      double normalizingFactor = 1.0 / boost::geometry::distance(rightPoints[i + 1], rightPoints[i]);
      Boost2dPoint normalizedDirection = direction;
      boost::geometry::multiply_value(normalizedDirection, normalizingFactor);

      orthogonalDirection.x(-normalizedDirection.y());
      orthogonalDirection.y(normalizedDirection.x());
      boost::geometry::multiply_value(orthogonalDirection, margin);
      boost::geometry::add_point(rightPoints[i], orthogonalDirection);
    }
    boost::geometry::add_point(rightPoints[i], orthogonalDirection);
  }

  std::vector<Boost2dPoint> points = leftPoints;
  // insert the right points in vice versa direction
  points.insert(points.end(), rightPoints.rbegin(), rightPoints.rend());
  // close polygon
  points.push_back(points.front());
  BoostPolygon polygon;
  ::boost::geometry::assign_points(polygon, points);
  // ensure that all polygons have the same orientation
  ::boost::geometry::correct(polygon);
  return polygon;
}

void invertLane(Lane &lane)
{
  std::reverse(lane.leftEdge.begin(), lane.leftEdge.end());
  std::reverse(lane.rightEdge.begin(), lane.rightEdge.end());
  std::swap(lane.leftEdge, lane.rightEdge);
  std::swap(lane.leftNeighbor, lane.rightNeighbor);
  std::swap(lane.predecessors, lane.successors);
  lane.index = -lane.index;

  for (auto &signalReference : lane.signalReferences)
  {
    signalReference.inLaneOrientation = !signalReference.inLaneOrientation;
    signalReference.parametricPosition = 1.0 - signalReference.parametricPosition;
  }

  for (auto &parametricSpeed : lane.speed)
  {
    auto start = 1.0 - parametricSpeed.end;
    auto end = 1.0 - parametricSpeed.start;
    parametricSpeed.end = end;
    parametricSpeed.start = start;
  }
}

void invertLaneAndNeighbors(LaneMap &laneMap, Lane &lane)
{
  std::set<Id> neighborhood;
  neighborhood.insert(lane.id);
  auto leftNeighbor = lane.leftNeighbor;
  while (leftNeighbor != 0u)
  {
    auto insertResult = neighborhood.insert(leftNeighbor);
    if (insertResult.second)
    {
      leftNeighbor = laneMap[leftNeighbor].leftNeighbor;
    }
    else
    {
      spdlog::error("invertLaneAndNeighbors( {} ) recursion", lane.id);
      leftNeighbor = 0u;
    }
  }
  auto rightNeighbor = lane.rightNeighbor;
  while (rightNeighbor != 0u)
  {
    auto insertResult = neighborhood.insert(rightNeighbor);
    if (insertResult.second)
    {
      rightNeighbor = laneMap[rightNeighbor].rightNeighbor;
    }
    else
    {
      spdlog::error("invertLaneAndNeighbors( {} ) recursion", lane.id);
      rightNeighbor = 0u;
    }
  }
  for (auto const &laneId : neighborhood)
  {
    invertLane(laneMap[laneId]);
  }
}

ContactPlace contactPlace(Lane const &leftLane, Lane const &rightLane)
{
  if (leftLane.leftEdge.empty() || leftLane.rightEdge.empty())
  {
    spdlog::error("Empty left lane {}", leftLane.id);
    return ContactPlace::None;
  }
  if (rightLane.leftEdge.empty() || rightLane.rightEdge.empty())
  {
    spdlog::error("Empty right lane {}", rightLane.id);
    return ContactPlace::None;
  }
  auto const thisLeftStart = leftLane.leftEdge.front();
  auto const thisRightStart = leftLane.rightEdge.front();
  auto const otherLeftStart = rightLane.leftEdge.front();
  auto const otherRightStart = rightLane.rightEdge.front();

  auto const thisLeftEnd = leftLane.leftEdge.back();
  auto const thisRightEnd = leftLane.rightEdge.back();
  auto const otherLeftEnd = rightLane.leftEdge.back();
  auto const otherRightEnd = rightLane.rightEdge.back();

  if ((near(thisLeftStart, otherLeftStart) && near(thisLeftEnd, otherLeftEnd))
      || (near(thisRightStart, otherRightStart) && near(thisRightEnd, otherRightEnd)))
  {
    return ContactPlace::Overlap;
  }
  else if (near(thisLeftStart, otherRightStart) && near(thisLeftEnd, otherRightEnd))
  {
    return ContactPlace::LeftRight;
  }
  else if (near(thisLeftStart, otherLeftEnd) && near(thisLeftEnd, otherLeftStart))
  {
    return ContactPlace::LeftLeft;
  }
  else if (near(thisRightStart, otherLeftStart) && near(thisRightEnd, otherLeftEnd))
  {
    return ContactPlace::RightLeft;
  }
  else if (near(thisRightStart, otherRightEnd) && near(thisRightEnd, otherRightStart))
  {
    return ContactPlace::RightRight;
  }

  return ContactPlace::None;
}

void checkAddPredecessor(Lane &lane, Lane const &otherLane)
{
  auto const thisLeftStart = lane.leftEdge.front();
  auto const thisRightStart = lane.rightEdge.front();
  auto const otherLeftStart = otherLane.leftEdge.front();
  auto const otherRightStart = otherLane.rightEdge.front();
  auto const otherLeftEnd = otherLane.leftEdge.back();
  auto const otherRightEnd = otherLane.rightEdge.back();

  if ((near(thisLeftStart, otherLeftEnd) && near(thisRightStart, otherRightEnd))
      || (near(thisLeftStart, otherRightStart) && near(thisRightStart, otherLeftStart)))
  {
    lane.predecessors.insert(otherLane.id);
  }
  else
  {
    spdlog::debug("checkAddPredecessor[ {} ] rejecting other lane: {}", lane.id, otherLane.id);
  }
}

void checkAddSuccessor(Lane &lane, Lane const &otherLane)
{
  auto const thisLeftEnd = lane.leftEdge.back();
  auto const thisRightEnd = lane.rightEdge.back();
  auto const otherLeftStart = otherLane.leftEdge.front();
  auto const otherRightStart = otherLane.rightEdge.front();
  auto const otherLeftEnd = otherLane.leftEdge.back();
  auto const otherRightEnd = otherLane.rightEdge.back();

  if ((near(thisLeftEnd, otherLeftStart) && near(thisRightEnd, otherRightStart))
      || (near(thisLeftEnd, otherRightEnd) && near(thisRightEnd, otherLeftEnd)))
  {
    lane.successors.insert(otherLane.id);
  }
  else
  {
    spdlog::debug("checkAddSuccessor[ {} ] rejecting other lane: {}", lane.id, otherLane.id);
  }
}

Id laneId(int roadId, int laneSectionIndex, int laneIndex)
{
  if (laneSectionIndex < 0)
  {
    spdlog::error("Invalid lane section index {}", laneSectionIndex);
  }

  return laneId(roadId, static_cast<uint64_t>(laneSectionIndex), laneIndex);
}

Id laneId(int roadId, uint64_t laneSectionIndex, int laneIndex)
{
  if (roadId < 0)
  {
    spdlog::error("Invalid road Id {}", roadId);
  }

  Id road = static_cast<Id>(roadId) * 10000;
  Id lane = static_cast<Id>(laneIndex) + 50;
  Id laneSection = static_cast<Id>(laneSectionIndex) * 100;

  return road + laneSection + lane;
}

bool lanesOverlap(Lane const &leftLane, Lane const &rightLane, double const overlapMargin)
{
  if (leftLane.id == rightLane.id)
  {
    return true;
  }
  if ((leftLane.type != LaneType::Driving) || (rightLane.type != LaneType::Driving))
  {
    return false;
  }

  try
  {
    auto leftPolygon = fromLane(leftLane, overlapMargin);
    auto rightPolygon = fromLane(rightLane, overlapMargin);
    std::deque<BoostPolygon> output;
    (void)::boost::geometry::intersection(leftPolygon, rightPolygon, output);
    return (!output.empty());
  }
  catch (...)
  {
    spdlog::error("Error while checking overlap between lanes {} and {} ", leftLane.id, rightLane.id);
    return false;
  }
}
} // namespace geometry
} // namespace opendrive


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief complex.cpp

\details
Demonstrate a complex number class that functions correctly with quantities.

Output:
@verbatim

//[complex_output_1
+L      = 2 + 1 i m
-L      = -2 + -1 i m
L+L     = 4 + 2 i m
L-L     = 0 + 0 i m
L*L     = 3 + 4 i m^2
L/L     = 1 + 0 i dimensionless
L^3     = 2 + 11 i m^3
L^(3/2) = 2.56713 + 2.14247 i m^(3/2)
3vL     = 1.29207 + 0.201294 i m^(1/3)
(3/2)vL = 1.62894 + 0.520175 i m^(2/3)
//]

//[complex_output_2
+L      = 2 m + 1 m i
-L      = -2 m + -1 m i
L+L     = 4 m + 2 m i
L-L     = 0 m + 0 m i
L*L     = 3 m^2 + 4 m^2 i
L/L     = 1 dimensionless + 0 dimensionless i
L^3     = 2 m^3 + 11 m^3 i
L^(3/2) = 2.56713 m^(3/2) + 2.14247 m^(3/2) i
3vL     = 1.29207 m^(1/3) + 0.201294 m^(1/3) i
(3/2)vL = 1.62894 m^(2/3) + 0.520175 m^(2/3) i
//]

@endverbatim
**/

#include <cmath>
#include <complex>
#include <iostream>

#include <boost/mpl/list.hpp>

#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>

#include "test_system.hpp"

//[complex_class_snippet_1
namespace boost {

namespace units {

/// replacement complex class
template<class T>
class complex
{
    public:
        typedef complex<T>  this_type;

        constexpr complex(const T& r = 0,const T& i = 0) : r_(r),i_(i) { }
        constexpr complex(const this_type& source) : r_(source.r_),i_(source.i_) { }

        constexpr this_type& operator=(const this_type& source)
        {
            if (this == &source) return *this;

            r_ = source.r_;
            i_ = source.i_;

            return *this;
        }

        constexpr T& real()             { return r_; }
        constexpr T& imag()             { return i_; }

        constexpr const T& real() const       { return r_; }
        constexpr const T& imag() const       { return i_; }

        constexpr this_type& operator+=(const T& val)
        {
            r_ += val;
            return *this;
        }

        constexpr this_type& operator-=(const T& val)
        {
            r_ -= val;
            return *this;
        }

        constexpr this_type& operator*=(const T& val)
        {
            r_ *= val;
            i_ *= val;
            return *this;
        }

        constexpr this_type& operator/=(const T& val)
        {
            r_ /= val;
            i_ /= val;
            return *this;
        }

        constexpr this_type& operator+=(const this_type& source)
        {
            r_ += source.r_;
            i_ += source.i_;
            return *this;
        }

        constexpr this_type& operator-=(const this_type& source)
        {
            r_ -= source.r_;
            i_ -= source.i_;
            return *this;
        }

        constexpr this_type& operator*=(const this_type& source)
        {
            *this = *this * source;
            return *this;
        }

        constexpr this_type& operator/=(const this_type& source)
        {
            *this = *this / source;
            return *this;
        }

    private:
        T   r_,i_;
};

}

}

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::complex, 1)

#endif

namespace boost {

namespace units {

template<class X>
constexpr
complex<typename unary_plus_typeof_helper<X>::type>
operator+(const complex<X>& x)
{
    typedef typename unary_plus_typeof_helper<X>::type  type;

    return complex<type>(x.real(),x.imag());
}

template<class X>
constexpr
complex<typename unary_minus_typeof_helper<X>::type>
operator-(const complex<X>& x)
{
    typedef typename unary_minus_typeof_helper<X>::type type;

    return complex<type>(-x.real(),-x.imag());
}

template<class X,class Y>
constexpr
complex<typename add_typeof_helper<X,Y>::type>
operator+(const complex<X>& x,const complex<Y>& y)
{
    typedef typename boost::units::add_typeof_helper<X,Y>::type type;

    return complex<type>(x.real()+y.real(),x.imag()+y.imag());
}

template<class X,class Y>
constexpr
complex<typename boost::units::subtract_typeof_helper<X,Y>::type>
operator-(const complex<X>& x,const complex<Y>& y)
{
    typedef typename boost::units::subtract_typeof_helper<X,Y>::type    type;

    return complex<type>(x.real()-y.real(),x.imag()-y.imag());
}

template<class X,class Y>
constexpr
complex<typename boost::units::multiply_typeof_helper<X,Y>::type>
operator*(const complex<X>& x,const complex<Y>& y)
{
    typedef typename boost::units::multiply_typeof_helper<X,Y>::type    type;

    return complex<type>(x.real()*y.real() - x.imag()*y.imag(),
                         x.real()*y.imag() + x.imag()*y.real());

//  fully correct implementation has more complex return type
//
//    typedef typename boost::units::multiply_typeof_helper<X,Y>::type xy_type;
//
//    typedef typename boost::units::add_typeof_helper<
//      xy_type,xy_type>::type         xy_plus_xy_type;
//    typedef typename
//        boost::units::subtract_typeof_helper<xy_type,xy_type>::type
//        xy_minus_xy_type;
//
//    BOOST_STATIC_ASSERT((boost::is_same<xy_plus_xy_type,
//                                       xy_minus_xy_type>::value == true));
//
//    return complex<xy_plus_xy_type>(x.real()*y.real()-x.imag()*y.imag(),
//                                    x.real()*y.imag()+x.imag()*y.real());
}

template<class X,class Y>
constexpr
complex<typename boost::units::divide_typeof_helper<X,Y>::type>
operator/(const complex<X>& x,const complex<Y>& y)
{
    // naive implementation of complex division
    typedef typename boost::units::divide_typeof_helper<X,Y>::type type;

    return complex<type>((x.real()*y.real()+x.imag()*y.imag())/
                            (y.real()*y.real()+y.imag()*y.imag()),
                         (x.imag()*y.real()-x.real()*y.imag())/
                            (y.real()*y.real()+y.imag()*y.imag()));

//  fully correct implementation has more complex return type
//
//  typedef typename boost::units::multiply_typeof_helper<X,Y>::type xy_type;
//  typedef typename boost::units::multiply_typeof_helper<Y,Y>::type yy_type;
//
//  typedef typename boost::units::add_typeof_helper<xy_type, xy_type>::type
//      xy_plus_xy_type;
//  typedef typename boost::units::subtract_typeof_helper<
//      xy_type,xy_type>::type xy_minus_xy_type;
//
//  typedef typename boost::units::divide_typeof_helper<
//      xy_plus_xy_type,yy_type>::type      xy_plus_xy_over_yy_type;
//  typedef typename boost::units::divide_typeof_helper<
//      xy_minus_xy_type,yy_type>::type     xy_minus_xy_over_yy_type;
//
//  BOOST_STATIC_ASSERT((boost::is_same<xy_plus_xy_over_yy_type,
//                                  xy_minus_xy_over_yy_type>::value == true));
//
//  return complex<xy_plus_xy_over_yy_type>(
//      (x.real()*y.real()+x.imag()*y.imag())/
//          (y.real()*y.real()+y.imag()*y.imag()),
//      (x.imag()*y.real()-x.real()*y.imag())/
//          (y.real()*y.real()+y.imag()*y.imag()));
}

template<class Y>
complex<Y>
pow(const complex<Y>& x,const Y& y)
{
    std::complex<Y> tmp(x.real(),x.imag());

    tmp = std::pow(tmp,y);

    return complex<Y>(tmp.real(),tmp.imag());
}

template<class Y>
std::ostream& operator<<(std::ostream& os,const complex<Y>& val)
{
    os << val.real() << " + " << val.imag() << " i";

    return os;
}

/// specialize power typeof helper for complex<Y>
template<class Y,long N,long D>
struct power_typeof_helper<complex<Y>,static_rational<N,D> >
{
    typedef complex<
        typename power_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const complex<Y>& x)
    {
        const static_rational<N,D>  rat;

        const Y    m = Y(rat.numerator())/Y(rat.denominator());

        return boost::units::pow(x,m);
    }
};

/// specialize root typeof helper for complex<Y>
template<class Y,long N,long D>
struct root_typeof_helper<complex<Y>,static_rational<N,D> >
{
    typedef complex<
        typename root_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const complex<Y>& x)
    {
        const static_rational<N,D>  rat;

        const Y    m = Y(rat.denominator())/Y(rat.numerator());

        return boost::units::pow(x,m);
    }
};

/// specialize power typeof helper for complex<quantity<Unit,Y> >
template<class Y,class Unit,long N,long D>
struct power_typeof_helper<complex<quantity<Unit,Y> >,static_rational<N,D> >
{
    typedef typename
        power_typeof_helper<Y,static_rational<N,D> >::type       value_type;
    typedef typename
        power_typeof_helper<Unit,static_rational<N,D> >::type    unit_type;
    typedef quantity<unit_type,value_type>                      quantity_type;
    typedef complex<quantity_type>                              type;

    static type value(const complex<quantity<Unit,Y> >& x)
    {
        const complex<value_type>   tmp =
            pow<static_rational<N,D> >(complex<Y>(x.real().value(),
                                                  x.imag().value()));

        return type(quantity_type::from_value(tmp.real()),
                    quantity_type::from_value(tmp.imag()));
    }
};

/// specialize root typeof helper for complex<quantity<Unit,Y> >
template<class Y,class Unit,long N,long D>
struct root_typeof_helper<complex<quantity<Unit,Y> >,static_rational<N,D> >
{
    typedef typename
        root_typeof_helper<Y,static_rational<N,D> >::type       value_type;
    typedef typename
        root_typeof_helper<Unit,static_rational<N,D> >::type    unit_type;
    typedef quantity<unit_type,value_type>                      quantity_type;
    typedef complex<quantity_type>                              type;

    static type value(const complex<quantity<Unit,Y> >& x)
    {
        const complex<value_type>   tmp =
            root<static_rational<N,D> >(complex<Y>(x.real().value(),
                                                   x.imag().value()));

        return type(quantity_type::from_value(tmp.real()),
                   quantity_type::from_value(tmp.imag()));
    }
};

} // namespace units

} // namespace boost
//]

int main(void)
{
    using namespace boost::units;
    using namespace boost::units::test;

    {
    //[complex_snippet_1
    typedef quantity<length,complex<double> >     length_dimension;

    const length_dimension    L(complex<double>(2.0,1.0)*meters);
    //]

    std::cout << "+L      = " << +L << std::endl
              << "-L      = " << -L << std::endl
              << "L+L     = " << L+L << std::endl
              << "L-L     = " << L-L << std::endl
              << "L*L     = " << L*L << std::endl
              << "L/L     = " << L/L << std::endl
              << "L^3     = " << pow<3>(L) << std::endl
              << "L^(3/2) = " << pow< static_rational<3,2> >(L) << std::endl
              << "3vL     = " << root<3>(L) << std::endl
              << "(3/2)vL = " << root< static_rational<3,2> >(L) << std::endl
              << std::endl;
    }

    {
    //[complex_snippet_2
    typedef complex<quantity<length> >     length_dimension;

    const length_dimension    L(2.0*meters,1.0*meters);
    //]

    std::cout << "+L      = " << +L << std::endl
              << "-L      = " << -L << std::endl
              << "L+L     = " << L+L << std::endl
              << "L-L     = " << L-L << std::endl
              << "L*L     = " << L*L << std::endl
              << "L/L     = " << L/L << std::endl
              << "L^3     = " << pow<3>(L) << std::endl
              << "L^(3/2) = " << pow< static_rational<3,2> >(L) << std::endl
              << "3vL     = " << root<3>(L) << std::endl
              << "(3/2)vL = " << root< static_rational<3,2> >(L) << std::endl
              << std::endl;
    }

    return 0;
}