/**
 * GeoDa TM, Copyright (C) 2011-2015 by Luc Anselin - all rights reserved
 *
 * This file is part of GeoDa.
 *
 * GeoDa is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GeoDa is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <math.h>
#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include "../logger.h"
#include "GdaFlexValue.h"

GdaFlexValue::GdaFlexValue()
	: V(1), obs(1), tms(1), is_string_lit(false),
	  weights_uuid(boost::uuids::nil_uuid())
{
	V = 0.0;
}

GdaFlexValue::GdaFlexValue(size_t obs_, size_t tms_)
	: V(obs_*tms_), obs(obs_), tms(tms_), is_string_lit(false),
	  weights_uuid(boost::uuids::nil_uuid())
{
	V = 0.0;
}

GdaFlexValue::GdaFlexValue(const std::valarray<double>& V_,
						   size_t obs_, size_t tms_)
	: V(V_), obs(obs_), tms(tms_), is_string_lit(false),
	  weights_uuid(boost::uuids::nil_uuid())
{
}

GdaFlexValue::GdaFlexValue(const std::vector<long>& counts)
	: V(counts.size()), obs(counts.size()), tms(1), is_string_lit(false),
	  weights_uuid(boost::uuids::nil_uuid())
{
	for (size_t i=0, sz=counts.size(); i<sz; ++i) {
		V[i] = (double) counts[i];
	}
}

GdaFlexValue::GdaFlexValue(const double& value)
	: V(value, 1), obs(1), tms(1), is_string_lit(false),
	  weights_uuid(boost::uuids::nil_uuid())
{
}

GdaFlexValue::GdaFlexValue(const GdaFlexValue& v)
	: V(v.V), obs(v.obs), tms(v.tms),
	  is_string_lit(v.is_string_lit), string_lit(v.string_lit),
	  weights_uuid(v.weights_uuid)
{
}

GdaFlexValue::GdaFlexValue(boost::uuids::uuid u)
	: V(0), obs(0), tms(1), is_string_lit(false), weights_uuid(u)
{
}

GdaFlexValue::GdaFlexValue(const wxString& str_lit)
	: V(0), obs(0), tms(1), is_string_lit(true), string_lit(str_lit)
{
	LOG(string_lit);
}

GdaFlexValue& GdaFlexValue::operator=(const GdaFlexValue& v)
{
	if (v.V.size() != V.size()) V.resize(v.V.size());
	V = v.V;
	obs = v.obs;
	tms = v.tms;
	is_string_lit = v.is_string_lit;
	string_lit = v.string_lit;
	weights_uuid = v.weights_uuid;
	return *this;
}

bool GdaFlexValue::IsStringLit() const
{
	return is_string_lit;
}

bool GdaFlexValue::IsData() const
{
	return weights_uuid.is_nil()
		&& !is_string_lit;
}

bool GdaFlexValue::IsWeights() const
{
	return !weights_uuid.is_nil() && !is_string_lit;
}

wxString GdaFlexValue::ToStr() const
{
	wxString s;
	if (IsStringLit()) {
		s << "string literal type\n";
		s << "string_lit: " << string_lit;
		return s;
	}
	if (IsWeights()) {
		s << "weights type\n";
		s << "weights_uuid: " << wxString(boost::uuids::to_string(weights_uuid));
		return s;
	}
	s << "data type\n";
	if (V.size() == 0) return s;
	if (V.size() == 1) {
		s << V[0];
	} else {
		for (size_t r=0; r<obs; ++r) {
			s << "[";
			for (size_t t=0; t<tms-1; ++t) {
				s << V[r*tms+t] << ", ";
			}
			s << V[r*tms+tms-1] << "]";
			if (r<obs-1) s << "\n";
		}
	}
	return s;
}
GdaFlexValue& GdaFlexValue::operator+=(const GdaFlexValue& v)
{
	exception_if_not_data(*this);
	exception_if_not_data(v);
	grow_if_smaller(v);
	// obs >= v.obs && tms >= v.tms
	if (obs == v.obs && tms == v.tms) {
		V += v.V;
	} else if (v.V.size() == 1) {
		V += v.V[0];
	} else if (tms > 1 && v.tms == 1) {
		for (size_t t=0; t<tms; ++t) {
			V[std::slice(t,obs,tms)] += v.V;
		}
	} else if (obs > 1 && v.obs == 1) {
		for (size_t i=0; i<obs; ++i) {
			V[std::slice(i*tms,tms,1)] += v.V;
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::operator-=(const GdaFlexValue& v)
{
	exception_if_not_data(*this);
	exception_if_not_data(v);
	grow_if_smaller(v);
	if (obs == v.obs && tms == v.tms) {
		V -= v.V;
	} else if (v.V.size() == 1) {
		V -= v.V[0];
	} else if (tms > 1 && v.tms == 1) {
		for (size_t t=0; t<tms; ++t) {
			V[std::slice(t,obs,tms)] -= v.V;
		}
	} else if (obs > 1 && v.obs == 1) {
		for (size_t i=0; i<obs; ++i) {
			V[std::slice(i*tms,tms,1)] -= v.V;
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::operator*=(const GdaFlexValue& v)
{
	exception_if_not_data(*this);
	exception_if_not_data(v);
	grow_if_smaller(v);
	if (obs == v.obs && tms == v.tms) {
		V *= v.V;
	} else if (v.V.size() == 1) {
		V *= v.V[0];
	} else if (tms > 1 && v.tms == 1) {
		for (size_t t=0; t<tms; ++t) {
			V[std::slice(t,obs,tms)] *= v.V;
		}
	} else if (obs > 1 && v.obs == 1) {
		for (size_t i=0; i<obs; ++i) {
			V[std::slice(i*tms,tms,1)] *= v.V;
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::operator/=(const GdaFlexValue& v)
{
	exception_if_not_data(*this);
	exception_if_not_data(v);
	grow_if_smaller(v);
	if (obs == v.obs && tms == v.tms) {
		V /= v.V;
	} else if (v.V.size() == 1) {
		V /= v.V[0];
	} else if (tms > 1 && v.tms == 1) {
		for (size_t t=0; t<tms; ++t) {
			V[std::slice(t,obs,tms)] /= v.V;
		}
	} else if (obs > 1 && v.obs == 1) {
		for (size_t i=0; i<obs; ++i) {
			V[std::slice(i*tms,tms,1)] /= v.V;
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::operator^=(const GdaFlexValue& v)
{
	exception_if_not_data(*this);
	exception_if_not_data(v);
	grow_if_smaller(v);
	if (obs == v.obs && tms == v.tms) {
		V = pow(V, v.V);
	} else if (v.V.size() == 1) {
		V = pow(V, v.V[0]);
	} else if (tms > 1 && v.tms == 1) {
		for (size_t t=0; t<tms; ++t) {
			V[std::slice(t,obs,tms)] =
				pow(std::valarray<double>(V[std::slice(t,obs,tms)]), v.V);
		}
	} else if (obs > 1 && v.obs == 1) {
		for (size_t i=0; i<obs; ++i) {
			V[std::slice(i*tms,tms,1)] =
				pow(std::valarray<double>(V[std::slice(i*tms,tms,1)]), v.V);
		}
	}

	return *this;
}

GdaFlexValue& GdaFlexValue::NegateSelf()
{
	exception_if_not_data(*this);
	V = -V;
	return *this;
}

GdaFlexValue& GdaFlexValue::ApplyUniSelf(double (*f)(double))
{
	LOG_MSG("In GdaFlexValue::ApplyUniSelf");
	exception_if_not_data(*this);
	for (int i=0, sz=V.size(); i<sz; ++i) V[i] = f(V[i]);
	return *this;
}

GdaFlexValue& GdaFlexValue::ApplyBinarySelf(double (*f)(double, double),
											const GdaFlexValue& v)
{
	LOG_MSG("In GdaFlexValue::ApplyBinarySelf");
	exception_if_not_data(*this);
	exception_if_not_data(v);
	grow_if_smaller(v);
	GdaFlexValue cpy_v(v);
	cpy_v.grow_if_smaller(*this);
	for (int i=0, sz=V.size(); i<sz; ++i) V[i] = f(V[i], cpy_v.V[i]);
	return *this;
}

GdaFlexValue& GdaFlexValue::Sum()
{
	exception_if_not_data(*this);
	if (obs == 1) return *this;
	std::valarray<double> x(0.0, tms);
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			x[t] += V[i*tms+t];
		}
	}
	V = x;
	obs = 1;
	return *this;
}

GdaFlexValue& GdaFlexValue::Mean()
{
	exception_if_not_data(*this);
	std::valarray<double> x(0.0, tms);
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			x[t] += V[i*tms+t];
		}
		x[t] /= obs;
	}
	V = x;
	obs = 1;
	return *this;
}

GdaFlexValue& GdaFlexValue::StdDev()
{
	exception_if_not_data(*this);
	std::valarray<double> mean(0.0, tms);
	std::valarray<double> sum_sq(0.0, tms);
	std::valarray<double> sd(0.0, tms);
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			mean[t] += V[i*tms+t];
		}
		mean[t] /= obs;
		for (size_t i=0; i<obs; ++i) {
			V[i*tms+t] -= mean[t];
			sum_sq[t] += V[i*tms+t] * V[i*tms+t];
		}
		sd[t] = sqrt(sum_sq[t] / (((double) obs)-1.0));
	}
	V = sd;
	obs = 1;
	return *this;
}

GdaFlexValue& GdaFlexValue::DevFromMean()
{
	exception_if_not_data(*this);
	std::valarray<double> mean(0.0, tms);
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			mean[t] += V[i*tms+t];
		}
		mean[t] /= obs;
		for (size_t i=0; i<obs; ++i) {
			V[i*tms+t] -= mean[t];
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::Standardize()
{
	exception_if_not_data(*this);
	std::valarray<double> mean(0.0, tms);
	std::valarray<double> sum_sq(0.0, tms);
	std::valarray<double> sd(0.0, tms);
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			mean[t] += V[i*tms+t];
		}
		mean[t] /= obs;
		for (size_t i=0; i<obs; ++i) {
			V[i*tms+t] -= mean[t];
			sum_sq[t] += V[i*tms+t] * V[i*tms+t];
		}
		sd[t] = sqrt(sum_sq[t] / (((double) obs)-1.0));
		for (size_t i=0; i<obs; ++i) {
			V[i*tms+t] /= sd[t];
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::Shuffle()
{
	exception_if_not_data(*this);
    // Mersenne Twister random number generator, randomly seeded
	// with current time in seconds since Jan 1 1970.
	static boost::mt19937 rng(std::time(0));
	static boost::random::uniform_int_distribution<> X(0, obs-1);
	// X(rng) -> returns a uniform random number from 0 to obs-1;
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			// swap each item in data with a random position in data.
			// This will produce a random permutation
			size_t r = (size_t) X(rng);
			double tmp = V[r*tms+t];
			V[r*tms+t] = V[i*tms+t];
			V[i*tms+t] = tmp;
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::UniformDist()
{
	exception_if_not_data(*this);
	static boost::mt19937 rng(std::time(0));
	static boost::uniform_01<boost::mt19937> X(rng);
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			V[i*tms+t] = X();
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::GaussianDist(double mean, double sd)
{
	exception_if_not_data(*this);
	static boost::mt19937 rng(std::time(0));
	boost::normal_distribution<> norm_dist(mean, sd);
	boost::variate_generator<boost::mt19937&,
							 boost::normal_distribution<> >
		X(rng, norm_dist);
	for (size_t t=0; t<tms; ++t) {
		for (size_t i=0; i<obs; ++i) {
			V[i*tms+t] = X();
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::Enumerate(double from, double step)
{
	exception_if_not_data(*this);
	for (size_t t=0; t<tms; ++t) {
		double x=from;
		for (size_t i=0; i<obs; ++i) {
			V[i*tms+t] = x;
			x+=step;
		}
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::Round()
{
	exception_if_not_data(*this);
	for (size_t i=0, sz=V.size(); i<sz; ++i) {
		V[i] = boost::math::round(V[i]);
	}
	return *this;
}

GdaFlexValue& GdaFlexValue::Max()
{
	exception_if_not_data(*this);
	if (obs == 1) return *this;
	double pos_inf = std::numeric_limits<double>::infinity();
	double nan = std::numeric_limits<double>::quiet_NaN();
	std::valarray<double> x(nan, tms);
	for (size_t t=0; t<tms; ++t) {
		bool any_defined = false;
		double m = -pos_inf;
		for (size_t i=0; i<obs; ++i) {
			double v = V[i*tms+t];
			if (v != v) continue;
			any_defined = true;
			if (v > m) m = v;
		}
		if (any_defined) x[t] = m;
	}
	V = x;
	obs = 1;
	return *this;
}

GdaFlexValue& GdaFlexValue::Min()
{
	exception_if_not_data(*this);
	if (obs == 1) return *this;
	double pos_inf = std::numeric_limits<double>::infinity();
	double nan = std::numeric_limits<double>::quiet_NaN();
	std::valarray<double> x(nan, tms);
	for (size_t t=0; t<tms; ++t) {
		bool any_defined = false;
		double m = pos_inf;
		for (size_t i=0; i<obs; ++i) {
			double v = V[i*tms+t];
			if (v != v) continue;
			any_defined = true;
			if (v < m) m = v;
		}
		if (any_defined) x[t] = m;
	}
	V = x;
	obs = 1;
	return *this;
}

GdaFlexValue& GdaFlexValue::Rotate(int step)
{
	exception_if_not_data(*this);
	for (size_t t=0; t<tms; ++t) {
		std::vector<double> tmp(obs);
		for (size_t i=0; i<obs; ++i) {
			tmp[i] = V[i*tms+t];
		}
		for (size_t i=0; i<obs; ++i) {
			int r = (((int) i)+step+obs) % ((int) obs);
			V[i*tms+t] = tmp[r];
		}
	}
	return *this;
}

double GdaFlexValue::GetDouble() const
{
	//exception_if_not_data(*this);
	return V.size() > 0 ? V[0] : std::numeric_limits<double>::quiet_NaN();
}

void GdaFlexValue::SetSize(size_t obs_, size_t tms_)
{
	if (obs == obs_ && tms == tms_) return;
	obs = obs_;
	tms = tms_;
	V.resize(obs_*tms_);
}

/**
  Increase the obs and tms dimensions to be compatible
  with the target value.

  Examples:

  A = [1]  B = [1]
               [2]
               [3]

  C = [1 2 3 4]  D = [1 2 3 4]
                     [5 6 7 8]
                     [9 10 11 12]

  A can grow to B, C or D
  B can grow to D
  C can grow to D
  B and C can not grow to each other's size,
  but rather they must grow to D's dimentions.

  eg B + C
  B -> [1 1 1 1]  C -> [1 2 3 4]
       [2 2 2 2]       [1 2 3 4]
       [3 3 3 3]       [1 2 3 4]

  B + C -> [2 3 4 5]
           [3 4 5 6]
           [4 5 6 7]

  B + mean(D)  in this case mean(D) collapses to the same
  dimensions as C

  So, increase dimensions does the following:
  increase this->obs if this->obs < v.obs
  increase this->tms if this->tms < v.tms

  Should be able to do this in a two step process. For
  example, to make A compatible with D
  [1] -> [1] -> [1 1 1 1]
         [1]    [1 1 1 1]
         [1]    [1 1 1 1]

  Enumerate will always become a single columm
  enumerate(from, step)

 */
void GdaFlexValue::grow_if_smaller(const GdaFlexValue& v)
{
	if (obs == v.obs && tms == v.tms) return;
	if (obs > 1 && v.obs > 1 && obs != v.obs) {
		throw GdaFVException("number of obs mismatch");
	}
	if (tms > 1 && v.tms > 1 && tms != v.tms) {
		throw GdaFVException("number of tms mismatch");
	}

	// in special case where V.size() == 1, can resize
    // much faster.
	if (V.size() == 1) {
		double t = V[0];
		V.resize(v.V.size());
		obs = v.obs;
		tms = v.tms;
		V = t;
		return;
	}

	// if *this has fewer obs, then increase obs dim
	if (obs < v.obs) {
		std::valarray<double> orig(V);
		V.resize(v.obs*tms);
		for (size_t i=0; i<v.obs; ++i) {
			V[std::slice(i*tms,tms,1)] = orig;
		}
		obs = v.obs;
	}

	// if *this has fewer tms, then increase tms dim
	if (tms < v.tms) {
		std::valarray<double> orig(V);
		V.resize(obs*v.tms);
		for (size_t t=0; t<v.tms; ++t) {
			V[std::slice(t,obs,v.tms)] = orig;
		}
		tms = v.tms;
	}
}

void GdaFlexValue::exception_if_not_data(const GdaFlexValue& x)
{
	if (!x.IsData()) {
		throw GdaFVException("value expected data expression");
	}
}

void GdaFlexValue::exception_if_not_weights(const GdaFlexValue& x)
{
	if (!x.IsWeights()) {
		throw GdaFVException("value expected weights expression");
	}
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "Odometry.h"

#include <Eigen/Dense>
#include <Core/Geometry/Image.h>
#include <Core/Geometry/RGBDImage.h>
#include <Core/Odometry/RGBDOdometryJacobian.h>
#include <Core/Utility/Eigen.h>
#include <Core/Utility/Timer.h>

namespace open3d {

namespace {

std::tuple<std::shared_ptr<Image>, std::shared_ptr<Image>>
        InitializeCorrespondenceMap(int width, int height)
{
    // initialization: filling with any (u,v) to (-1,-1)
    auto correspondence_map = std::make_shared<Image>();
    auto depth_buffer = std::make_shared<Image>();
    correspondence_map->PrepareImage(width, height, 2, 4);
    depth_buffer->PrepareImage(width, height, 1, 4);
    for (int v = 0; v < correspondence_map->height_; v++) {
        for (int u = 0; u < correspondence_map->width_; u++) {
            *PointerAt<int>(*correspondence_map, u, v, 0) = -1;
            *PointerAt<int>(*correspondence_map, u, v, 1) = -1;
            *PointerAt<float>(*depth_buffer, u, v, 0) = -1.0f;
        }
    }
    return std::make_tuple(correspondence_map, depth_buffer);
}

inline void AddElementToCorrespondenceMap(
        Image &correspondence_map, Image &depth_buffer,
        int u_s, int v_s, int u_t, int v_t, float transformed_d_t) {
    int exist_u_t, exist_v_t;
    double exist_d_t;
    exist_u_t = *PointerAt<int>(correspondence_map, u_s, v_s, 0);
    exist_v_t = *PointerAt<int>(correspondence_map, u_s, v_s, 1);
    if (exist_u_t != -1 && exist_v_t != -1) {
        exist_d_t = *PointerAt<float>(depth_buffer, u_s, v_s);
        if (transformed_d_t < exist_d_t) { // update nearer point as correspondence
            *PointerAt<int>(correspondence_map, u_s, v_s, 0) = u_t;
            *PointerAt<int>(correspondence_map, u_s, v_s, 1) = v_t;
            *PointerAt<float>(depth_buffer, u_s, v_s) = transformed_d_t;
        }
    } else { // register correspondence
        *PointerAt<int>(correspondence_map, u_s, v_s, 0) = u_t;
        *PointerAt<int>(correspondence_map, u_s, v_s, 1) = v_t;
        *PointerAt<float>(depth_buffer, u_s, v_s) = transformed_d_t;
    }
}

void MergeCorrespondenceMaps(
        Image &correspondence_map, Image &depth_buffer,
        Image &correspondence_map_part, Image &depth_buffer_part)
{
    for (int v_s = 0; v_s < correspondence_map.height_; v_s++) {
        for (int u_s = 0; u_s < correspondence_map.width_; u_s++) {
            int u_t = *PointerAt<int>(correspondence_map_part, u_s, v_s, 0);
            int v_t = *PointerAt<int>(correspondence_map_part, u_s, v_s, 1);
            if (u_t != -1 && v_t != -1) {
                float transformed_d_t = *PointerAt<float>(
                        depth_buffer_part, u_s, v_s);
                AddElementToCorrespondenceMap(correspondence_map,
                        depth_buffer, u_s, v_s, u_t, v_t, transformed_d_t);
            }
        }
    }
}

int CountCorrespondence(const Image &correspondence_map)
{
    int correspondence_count = 0;
    for (int v_s = 0; v_s < correspondence_map.height_; v_s++) {
        for (int u_s = 0; u_s < correspondence_map.width_; u_s++) {
            int u_t = *PointerAt<int>(correspondence_map, u_s, v_s, 0);
            int v_t = *PointerAt<int>(correspondence_map, u_s, v_s, 1);
            if (u_t != -1 && v_t != -1) {
                correspondence_count++;
            }
        }
    }
    return correspondence_count;
}

std::shared_ptr<CorrespondenceSetPixelWise> ComputeCorrespondence(
        const Eigen::Matrix3d intrinsic_matrix,
        const Eigen::Matrix4d &extrinsic,
        const Image &depth_s, const Image &depth_t,
        const OdometryOption &option)
{
    const Eigen::Matrix3d K = intrinsic_matrix;
    const Eigen::Matrix3d K_inv = K.inverse();
    const Eigen::Matrix3d R = extrinsic.block<3, 3>(0, 0);
    const Eigen::Matrix3d KRK_inv = K * R * K_inv;
    Eigen::Vector3d Kt = K * extrinsic.block<3, 1>(0, 3);

    std::shared_ptr<Image> correspondence_map;
    std::shared_ptr<Image> depth_buffer;
    std::tie(correspondence_map, depth_buffer) =
            InitializeCorrespondenceMap(depth_t.width_, depth_t.height_);

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
    std::shared_ptr<Image> correspondence_map_private;
    std::shared_ptr<Image> depth_buffer_private;
    std::tie(correspondence_map_private, depth_buffer_private) =
            InitializeCorrespondenceMap(depth_t.width_, depth_t.height_);
#ifdef _OPENMP
#pragma omp for nowait
#endif
    for (int v_s = 0; v_s < depth_s.height_; v_s++) {
        for (int u_s = 0; u_s < depth_s.width_; u_s++) {
            double d_s = *PointerAt<float>(depth_s, u_s, v_s);
            if (!std::isnan(d_s)) {
                Eigen::Vector3d uv_in_s =
                    d_s * KRK_inv * Eigen::Vector3d(u_s, v_s, 1.0) + Kt;
                double transformed_d_s = uv_in_s(2);
                int u_t = (int)(uv_in_s(0) / transformed_d_s + 0.5);
                int v_t = (int)(uv_in_s(1) / transformed_d_s + 0.5);
                if (u_t >= 0 && u_t < depth_t.width_ &&
                        v_t >= 0 && v_t < depth_t.height_) {
                    double d_t = *PointerAt<float>(depth_t, u_t, v_t);
                    if (!std::isnan(d_t) && std::abs(transformed_d_s - d_t)
                            <= option.max_depth_diff_) {
                        AddElementToCorrespondenceMap(
                                *correspondence_map_private,
                                *depth_buffer_private,
                                u_s, v_s, u_t, v_t, (float)d_s);
                    }
                }
            }
        }
    }
#ifdef _OPENMP
#pragma omp critical
{
#endif
    MergeCorrespondenceMaps(
            *correspondence_map, *depth_buffer,
            *correspondence_map_private, *depth_buffer_private);
#ifdef _OPENMP
}    //    omp critical
}    //    omp parallel
#endif

    auto correspondence = std::make_shared<CorrespondenceSetPixelWise>();
    int correspondence_count = CountCorrespondence(*correspondence_map);
    correspondence->resize(correspondence_count);
    int cnt = 0;
    for (int v_s = 0; v_s < correspondence_map->height_; v_s++) {
        for (int u_s = 0; u_s < correspondence_map->width_; u_s++) {
            int u_t = *PointerAt<int>(*correspondence_map, u_s, v_s, 0);
            int v_t = *PointerAt<int>(*correspondence_map, u_s, v_s, 1);
            if (u_t != -1 && v_t != -1) {
                Eigen::Vector4i pixel_correspondence(u_s, v_s, u_t, v_t);
                (*correspondence)[cnt] = pixel_correspondence;
                cnt++;
            }
        }
    }
    return correspondence;
}

std::shared_ptr<Image> ConvertDepthImageToXYZImage(
        const Image &depth, const Eigen::Matrix3d &intrinsic_matrix)
{
    auto image_xyz = std::make_shared<Image>();
    if (depth.num_of_channels_ != 1 || depth.bytes_per_channel_ != 4) {
        PrintDebug("[ConvertDepthImageToXYZImage] Unsupported image format.\n");
        return image_xyz;
    }
    const double inv_fx = 1.0 / intrinsic_matrix(0, 0);
    const double inv_fy = 1.0 / intrinsic_matrix(1, 1);
    const double ox = intrinsic_matrix(0, 2);
    const double oy = intrinsic_matrix(1, 2);
    image_xyz->PrepareImage(depth.width_, depth.height_, 3, 4);

    for (int y = 0; y < image_xyz->height_; y++) {
        for (int x = 0; x < image_xyz->width_; x++) {
            float *px = PointerAt<float>(*image_xyz, x, y, 0);
            float *py = PointerAt<float>(*image_xyz, x, y, 1);
            float *pz = PointerAt<float>(*image_xyz, x, y, 2);
            float z = *PointerAt<float>(depth, x, y);
            *px = (float)((x - ox) * z * inv_fx);
            *py = (float)((y - oy) * z * inv_fy);
            *pz = z;
        }
    }
    return image_xyz;
}

std::vector<Eigen::Matrix3d>
        CreateCameraMatrixPyramid(
        const PinholeCameraIntrinsic &pinhole_camera_intrinsic, int levels)
{
    std::vector<Eigen::Matrix3d> pyramid_camera_matrix;
    pyramid_camera_matrix.reserve(levels);
    for (int i = 0; i < levels; i++) {
        Eigen::Matrix3d level_camera_matrix;
        if (i == 0)
            level_camera_matrix = pinhole_camera_intrinsic.intrinsic_matrix_;
        else
            level_camera_matrix = 0.5 * pyramid_camera_matrix[i - 1];
        level_camera_matrix(2, 2) = 1.;
        pyramid_camera_matrix.push_back(level_camera_matrix);
    }
    return pyramid_camera_matrix;
}

Eigen::Matrix6d CreateInformationMatrix(
        const Eigen::Matrix4d &extrinsic,
        const PinholeCameraIntrinsic &pinhole_camera_intrinsic,
        const Image &depth_s, const Image &depth_t,
        const OdometryOption &option)
{
    auto correspondence = ComputeCorrespondence(
            pinhole_camera_intrinsic.intrinsic_matrix_,
            extrinsic, depth_s, depth_t, option);

    auto xyz_t = ConvertDepthImageToXYZImage(
            depth_t, pinhole_camera_intrinsic.intrinsic_matrix_);

    // write q^*
    // see http://redwood-data.org/indoor/registration.html
    // note: I comes first and q_skew is scaled by factor 2.
    Eigen::Matrix6d GTG = Eigen::Matrix6d::Identity();
#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        Eigen::Matrix6d GTG_private = Eigen::Matrix6d::Identity();
        Eigen::Vector6d G_r_private = Eigen::Vector6d::Zero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (auto row = 0; row < correspondence->size(); row++) {
            int u_t = (*correspondence)[row](2);
            int v_t = (*correspondence)[row](3);
            double x = *PointerAt<float>(*xyz_t, u_t, v_t, 0);
            double y = *PointerAt<float>(*xyz_t, u_t, v_t, 1);
            double z = *PointerAt<float>(*xyz_t, u_t, v_t, 2);
            G_r_private.setZero();
            G_r_private(1) = z;
            G_r_private(2) = -y;
            G_r_private(3) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = -z;
            G_r_private(2) = x;
            G_r_private(4) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = y;
            G_r_private(1) = -x;
            G_r_private(5) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            GTG += GTG_private;
        }
#ifdef _OPENMP
    }
#endif
    return std::move(GTG);
}

void NormalizeIntensity(Image &image_s, Image &image_t,
        CorrespondenceSetPixelWise &correspondence)
{
    if (image_s.width_ != image_t.width_ ||
        image_s.height_ != image_t.height_) {
        PrintError("[NormalizeIntensity] Size of two input images should be same\n");
        return;
    }
    double mean_s = 0.0, mean_t = 0.0;
    for (int row = 0; row < correspondence.size(); row++) {
        int u_s = correspondence[row](0);
        int v_s = correspondence[row](1);
        int u_t = correspondence[row](2);
        int v_t = correspondence[row](3);
        mean_s += *PointerAt<float>(image_s, u_s, v_s);
        mean_t += *PointerAt<float>(image_t, u_t, v_t);
    }
    mean_s /= (double)correspondence.size();
    mean_t /= (double)correspondence.size();
    LinearTransformImage(image_s, 0.5 / mean_s, 0.0);
    LinearTransformImage(image_t, 0.5 / mean_t, 0.0);
}

inline std::shared_ptr<RGBDImage> PackRGBDImage(
    const Image &color, const Image &depth) {
    return std::make_shared<RGBDImage>(RGBDImage(color, depth));
}

std::shared_ptr<Image> PreprocessDepth(
        const Image &depth_orig, const OdometryOption &option)
{
    std::shared_ptr<Image> depth_processed = std::make_shared<Image>();
    *depth_processed = depth_orig;
    for (int y = 0; y < depth_processed->height_; y++) {
        for (int x = 0; x < depth_processed->width_; x++) {
            float *p = PointerAt<float>(*depth_processed, x, y);
            if ((*p < option.min_depth_ || *p > option.max_depth_ || *p <= 0))
                *p = std::numeric_limits<float>::quiet_NaN();
        }
    }
    return depth_processed;
}

inline bool CheckImagePair(const Image &image_s, const Image &image_t)
{
    return (image_s.width_ == image_t.width_ &&
            image_s.height_ == image_t.height_);
}

inline bool CheckRGBDImagePair(const RGBDImage &source, const RGBDImage &target)
{
    return (CheckImagePair(source.color_, target.color_) &&
            CheckImagePair(source.depth_, target.depth_) &&
            CheckImagePair(source.color_, source.depth_) &&
            CheckImagePair(target.color_, target.depth_) &&
            CheckImagePair(source.color_, target.color_) &&
            source.color_.num_of_channels_ == 1 &&
            source.depth_.num_of_channels_ == 1 &&
            target.color_.num_of_channels_ == 1 &&
            target.depth_.num_of_channels_ == 1 &&
            source.color_.bytes_per_channel_ == 4 &&
            target.color_.bytes_per_channel_ == 4 &&
            source.depth_.bytes_per_channel_ == 4 &&
            target.depth_.bytes_per_channel_ == 4);
}

std::tuple<std::shared_ptr<RGBDImage>, std::shared_ptr<RGBDImage>>
        InitializeRGBDOdometry(
        const RGBDImage &source, const RGBDImage &target,
        const PinholeCameraIntrinsic &pinhole_camera_intrinsic,
        const Eigen::Matrix4d &odo_init,
        const OdometryOption &option)
{
    auto source_gray = FilterImage(source.color_,
            Image::FilterType::Gaussian3);
    auto target_gray = FilterImage(target.color_,
            Image::FilterType::Gaussian3);
    auto source_depth_preprocessed = PreprocessDepth(source.depth_, option);
    auto target_depth_preprocessed = PreprocessDepth(target.depth_, option);
    auto source_depth = FilterImage(*source_depth_preprocessed,
            Image::FilterType::Gaussian3);
    auto target_depth = FilterImage(*target_depth_preprocessed,
            Image::FilterType::Gaussian3);

    auto correspondence = ComputeCorrespondence(
            pinhole_camera_intrinsic.intrinsic_matrix_, odo_init,
            *source_depth, *target_depth, option);
    int corresps_count_required = (int)(source_gray->height_ *
            source_gray->width_ * option.minimum_correspondence_ratio_ + 0.5);
    if (correspondence->size() < corresps_count_required) {
        PrintWarning("[InitializeRGBDPair] Bad initial pose\n");
    }
    NormalizeIntensity(*source_gray, *target_gray, *correspondence);

    auto source_out = PackRGBDImage(*source_gray, *source_depth);
    auto target_out = PackRGBDImage(*target_gray, *target_depth);
    return std::make_tuple(source_out, target_out);
}

std::tuple<bool, Eigen::Matrix4d> DoSingleIteration(
    int iter, int level,
    const RGBDImage &source, const RGBDImage &target,
    const Image &source_xyz,
    const RGBDImage &target_dx, const RGBDImage &target_dy,
    const Eigen::Matrix3d intrinsic,
    const Eigen::Matrix4d &extrinsic_initial,
    const RGBDOdometryJacobian &jacobian_method,
    const OdometryOption &option)
{
    auto correspondence = ComputeCorrespondence(
            intrinsic, extrinsic_initial, source.depth_, target.depth_, option);
    int corresps_count_required = (int)(source.color_.height_ *
            source.color_.width_ * option.minimum_correspondence_ratio_ + 0.5);
    int corresps_count = (int)correspondence->size();
    if (corresps_count < corresps_count_required) {
        PrintWarning("[ComputeOdometry] Too fewer correspondences (%d found / %d required)\n",
                corresps_count, corresps_count_required);
        return std::make_tuple(false, Eigen::Matrix4d::Identity());
    }

    auto f_lambda = [&]
            (int i, std::vector<Eigen::Vector6d> &J_r, std::vector<double> &r) {
        jacobian_method.ComputeJacobianAndResidual(i, J_r, r,
                source, target, source_xyz, target_dx, target_dy,
                intrinsic, extrinsic_initial, *correspondence);
    };
    PrintDebug("Iter : %d, Level : %d, ", iter, level);
    Eigen::Matrix6d JTJ;
    Eigen::Vector6d JTr;
    std::tie(JTJ, JTr) = ComputeJTJandJTr<Eigen::Matrix6d, Eigen::Vector6d>(
            f_lambda, corresps_count);

    bool is_success;
    Eigen::Matrix4d extrinsic;
    std::tie(is_success, extrinsic) =
            SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);
    if (!is_success) {
        PrintWarning("[ComputeOdometry] no solution!\n");
        return std::make_tuple(false, Eigen::Matrix4d::Identity());
    } else {
        return std::make_tuple(true, extrinsic);
    }
}

std::tuple<bool, Eigen::Matrix4d> ComputeMultiscale(
        const RGBDImage &source, const RGBDImage &target,
        const PinholeCameraIntrinsic &pinhole_camera_intrinsic,
        const Eigen::Matrix4d &extrinsic_initial,
        const RGBDOdometryJacobian &jacobian_method,
        const OdometryOption &option)
{
    std::vector<int> iter_counts = option.iteration_number_per_pyramid_level_;
    int num_levels = (int)iter_counts.size();

    auto source_pyramid = CreateRGBDImagePyramid(source, num_levels);
    auto target_pyramid = CreateRGBDImagePyramid(target, num_levels);
    auto target_pyramid_dx = FilterRGBDImagePyramid
            (target_pyramid, Image::FilterType::Sobel3Dx);
    auto target_pyramid_dy = FilterRGBDImagePyramid
            (target_pyramid, Image::FilterType::Sobel3Dy);

    Eigen::Matrix4d result_odo = extrinsic_initial.isZero() ?
            Eigen::Matrix4d::Identity() : extrinsic_initial;

    std::vector<Eigen::Matrix3d> pyramid_camera_matrix =
            CreateCameraMatrixPyramid(pinhole_camera_intrinsic,
            (int)iter_counts.size());

    for (int level = num_levels - 1; level >= 0; level--) {
        const Eigen::Matrix3d level_camera_matrix = pyramid_camera_matrix[level];

        auto source_xyz_level = ConvertDepthImageToXYZImage(
                source_pyramid[level]->depth_, level_camera_matrix);
        auto source_level = PackRGBDImage(source_pyramid[level]->color_,
                source_pyramid[level]->depth_);
        auto target_level = PackRGBDImage(target_pyramid[level]->color_,
                target_pyramid[level]->depth_);
        auto target_dx_level = PackRGBDImage(target_pyramid_dx[level]->color_,
                target_pyramid_dx[level]->depth_);
        auto target_dy_level = PackRGBDImage(target_pyramid_dy[level]->color_,
                target_pyramid_dy[level]->depth_);

        for (int iter = 0; iter < iter_counts[num_levels - level - 1]; iter++) {
            Eigen::Matrix4d curr_odo;
            bool is_success;
            std::tie(is_success, curr_odo) = DoSingleIteration(
                iter, level,
                *source_level, *target_level, *source_xyz_level,
                *target_dx_level, *target_dy_level, level_camera_matrix,
                result_odo, jacobian_method, option);
            result_odo = curr_odo * result_odo;

            if (!is_success) {
                PrintWarning("[ComputeOdometry] no solution!\n");
                return std::make_tuple(false, Eigen::Matrix4d::Identity());
            }
        }
    }
    return std::make_tuple(true, result_odo);
}

}    // unnamed namespace

std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d>
        ComputeRGBDOdometry(const RGBDImage &source, const RGBDImage &target,
        const PinholeCameraIntrinsic &pinhole_camera_intrinsic
        /*= PinholeCameraIntrinsic()*/,
        const Eigen::Matrix4d &odo_init /*= Eigen::Matrix4d::Identity()*/,
        const RGBDOdometryJacobian &jacobian_method
        /*=RGBDOdometryJacobianFromHybridTerm*/,
        const OdometryOption &option /*= OdometryOption()*/)
{
    if (!CheckRGBDImagePair(source, target)) {
        PrintError("[RGBDOdometry] Two RGBD pairs should be same in size.\n");
        return std::make_tuple(false,
                Eigen::Matrix4d::Identity(), Eigen::Matrix6d::Zero());
    }

    std::shared_ptr<RGBDImage> source_processed, target_processed;
    std::tie(source_processed, target_processed) =
            InitializeRGBDOdometry(source, target, pinhole_camera_intrinsic,
            odo_init, option);

    Eigen::Matrix4d extrinsic;
    bool is_success;
    std::tie(is_success, extrinsic) = ComputeMultiscale(
            *source_processed, *target_processed,
            pinhole_camera_intrinsic, odo_init, jacobian_method, option);

    if (is_success) {
        Eigen::Matrix4d trans_output = extrinsic;
        Eigen::MatrixXd info_output = CreateInformationMatrix(extrinsic,
                pinhole_camera_intrinsic, source_processed->depth_,
                target_processed->depth_, option);
        return std::make_tuple(true, trans_output, info_output);
    }
    else {
        return std::make_tuple(false,
                Eigen::Matrix4d::Identity(), Eigen::Matrix6d::Identity());
    }
}

}    // namespace open3d


//==============================================================================
//
//   (c) Copyright, 2010 University Corporation for Atmospheric Research (UCAR).
//       All rights reserved.
//       Do not copy or distribute without authorization.
//
//       File: $RCSfile: ruc2wrfgrid.cc,v $
//       Version: $Revision: 1.2 $  Dated: $Date: 2010-05-05 19:29:17 $
//
//==============================================================================

/**
 * @file ruc2wrfgrid.cc
 *
 * Convert RUC 130 conus lat-longs to WRF grid coordinates
 *
 * @date 3/25/10
 */

// Include files
#include <stdio.h>
#include <string>
#include <vector>
#include <projects.h>
#include <boost/format.hpp>
#include "Proj4Wrap.hh"

using boost::format;
using std::string;
using std::vector;

// Functions

int main(int argc, char **argv)
{
  // Uses latitude where lambert conformal projection is true and
  // median aligned with cartesian y-axis
  double ruclov = -95;
  double rucla1 = 16.281;
  double ruclo1 = -126.138;
  double ruclatin1 = 25.0;
  double ruclatin2 = 25.0;
  double rucdx = 13545.087;
  double rucdy = 13545.087;
  int rucnx = 451;
  int rucny = 337;

  string rucParamString = str(format("+proj=lcc +R=6371200 +lon_0=%1% +lat_0=%2% +lat_1=%3% +lat_2=%4%") % ruclov % ruclatin1 % ruclatin1 % ruclatin2);

  p4w::Proj4Wrap rucLambertProj(rucParamString, p4w::Proj4Wrap::LON_LAT_TYPE, ruclo1, rucla1, rucdx, rucdy);
  printf("ruc false easting, false northing: %g %g\n", rucLambertProj.getFalseEasting(), rucLambertProj.getFalseNorthing());
  double xc;
  double yc;
  double lon;
  double lat;

  rucLambertProj.ll2xy(ruclo1, rucla1, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);
  rucLambertProj.ll2xy(-57.383, 55.481, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);
  rucLambertProj.xy2ll(0, 0, &lon, &lat);
  printf("lon, lat: %g %g\n", lon, lat);

  p4w::Proj4Wrap rucLambertProj1(rucParamString, p4w::Proj4Wrap::EASTING_NORTHING_TYPE, 3.33214e+06, 588890, rucdx, rucdy);
  printf("EASTINGNORTHING\n");
  printf("ruc lon lat: %g %g\n", rucLambertProj1.getOriginLon(), rucLambertProj1.getOriginLat());

  rucLambertProj1.ll2xy(ruclo1, rucla1, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);
  rucLambertProj1.ll2xy(-57.383, 55.481, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);
  rucLambertProj1.xy2ll(0, 0, &lon, &lat);
  printf("lon, lat: %g %g\n", lon, lat);

  p4w::Proj4Wrap rucLambertProj2(rucParamString, p4w::Proj4Wrap::EASTING_NORTHING_TYPE, 0, 0, rucdx, rucdy);
  printf("EASTINGNORTHING\n");
  printf("ruc lon lat: %g %g\n", rucLambertProj2.getOriginLon(), rucLambertProj2.getOriginLat());

  rucLambertProj2.ll2xy(ruclo1, rucla1, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);
  rucLambertProj2.ll2xy(-57.383, 55.481, &xc, &yc);
  printf("xc, yc: %g %g\n", xc, yc);
  rucLambertProj2.xy2ll(0, 0, &lon, &lat);
  printf("lon, lat: %g %g\n", lon, lat);

  double wrflov = -95;
  double wrfla1 = 1.066;
  double wrflo1 = -128.499;
  double wrflatin1 = 45.0;
  double wrflatin2 = 45.0;
  double wrfdx = 13545.087;
  double wrfdy = 13545.087;

  printf("\n");
  string wrfParamString = str(format("+proj=lcc +R=6370000 +lon_0=%1% +lat_1=%2% +lat_2=%3%") % wrflov % wrflatin1 % wrflatin2);

  p4w::Proj4Wrap wrfLambertProj(wrfParamString, p4w::Proj4Wrap::LON_LAT_TYPE, wrflo1, wrfla1, wrfdx, wrfdy);

  printf("netcdf rucwrf_coord {\n");
  printf("dimensions:\n");
  printf("\ty = %d;\n", rucny);
  printf("\tx = %d;\n", rucnx);
  printf("variables:\n");
  printf("float x(y, x);\n");
  printf("x:long_name = \"x coordinate\";\n");
  printf("float y(y, x);\n");
  printf("y:long_name = \"y coordinate\";\n");
  printf("data:\n");
  printf("x = \n");

  vector<float> xvec;
  vector<float> yvec;
  for (int i=0; i<337; i++)
    for (int j=0; j<451; j++)
      {
	double xc = j;
	double yc = i;
	double lat;
	double lon;
	double lat1;
	double lon1;

	rucLambertProj.xy2ll(xc, yc, &lon, &lat);
	wrfLambertProj.ll2xy(lon, lat, &xc, &yc);
	wrfLambertProj.xy2ll(xc, yc, &lon1, &lat1);

	if (fabs(lat-lat1) > 0.000001 || fabs(lon-lon1) > 0.000001)
	  printf("ERROR: lat diff, lon diff: %g %g\n", fabs(lat-lat1), fabs(lon-lon1));
	xvec.push_back(xc);
	yvec.push_back(yc);
      }

  int totalCt = rucnx * rucny;
  for (int i=0; i<totalCt-1; i++)
    {
      printf("%.7lf,\n", xvec[i]);
    }
  printf("%.7lf;\n", xvec[totalCt-1]);

  printf("y = \n");

  for (int i=0; i<totalCt-1; i++)
    {
      printf("%.7lf,\n", yvec[i]);
    }
  printf("%.7lf;\n", yvec[totalCt-1]);

  printf("}\n");
}


/*
 * Copyright (c) 2020 Andrew Price
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "mps_voxels/OccupancyData.h"
#include "mps_voxels/moveit_pose_type.h"
#include "mps_voxels/Scene.h"

#include "mps_voxels/LocalOctreeServer.h" // for setBBox()
#include "mps_voxels/shape_utils.h"
#include "mps_voxels/project_point.hpp"

#include <image_geometry/pinhole_camera_model.h>

#include <Eigen/StdVector>

// From https://tavianator.com/cgit/dimension.git/tree/libdimension/bvh/bvh.c#n196
// NB: Will miss if ray is coplanar with a box face
bool
fast_ray_box_intersection(const Eigen::Vector3f& rayOrigin,
                          const Eigen::Vector3f& rayVectorInv,
                          const Eigen::Vector3f& bboxMin,
                          const Eigen::Vector3f& bboxMax)
{
// This is actually correct, even though it appears not to handle edge cases
// (ray.n.{x,y,z} == 0).  It works because the infinities that result from
// dividing by zero will still behave correctly in the comparisons.  Rays
// which are parallel to an axis and outside the box will have tmin == inf
// or tmax == -inf, while rays inside the box will have tmin and tmax
// unchanged.

	double tx1 = (bboxMin.x() - rayOrigin.x())*rayVectorInv.x();
	double tx2 = (bboxMax.x() - rayOrigin.x())*rayVectorInv.x();

	double tmin = std::min(tx1, tx2);
	double tmax = std::max(tx1, tx2);

	double ty1 = (bboxMin.y() - rayOrigin.y())*rayVectorInv.y();
	double ty2 = (bboxMax.y() - rayOrigin.y())*rayVectorInv.y();

	tmin = std::max(tmin, std::min(ty1, ty2));
	tmax = std::min(tmax, std::max(ty1, ty2));

	double tz1 = (bboxMin.z() - rayOrigin.z())*rayVectorInv.z();
	double tz2 = (bboxMax.z() - rayOrigin.z())*rayVectorInv.z();

	tmin = std::max(tmin, std::min(tz1, tz2));
	tmax = std::min(tmax, std::max(tz1, tz2));

	return tmax >= std::max(0.0, tmin);
}

namespace mps
{

OccupancyData::OccupancyData(std::shared_ptr<VoxelRegion> region)
	: voxelRegion(std::move(region)),
	  vertexState(voxelRegion->num_vertices(), VoxelRegion::FREE_SPACE),
	  edgeState(voxelRegion->num_edges(), false)
{

}

OccupancyData::OccupancyData(std::shared_ptr<VoxelRegion> _region,
                             VoxelRegion::VertexLabels _state)
	: voxelRegion(std::move(_region)),
	  vertexState(std::move(_state)),
	  edgeState(voxelRegion->num_edges(), false),
	  uniqueObjectLabels(getUniqueObjectLabels(vertexState))
{
	// There's currently ~1.5M edges. Reserve this for a sparser state representation
	// Build the edge representation
//	for (VoxelRegion::edges_size_type e = 0; e < voxelRegion->num_edges(); ++e)
//	{
//		const VoxelRegion::edge_descriptor edge = voxelRegion->edge_at(e);
//		auto i = voxelRegion->index_of(source(edge, voxelRegion));
//		auto j = voxelRegion->index_of(target(edge, voxelRegion));
//
//		edgeState[e] = (vertexState[i] == vertexState[j]);
//	}

	// Allocate the object representation
	for (const auto& id : uniqueObjectLabels)
	{
		auto subtree = std::make_shared<octomap::OcTree>(voxelRegion->resolution);
		subtree->setProbMiss(0.05);
		subtree->setProbHit(0.95);
		setBBox(voxelRegion->regionMin, voxelRegion->regionMax, subtree.get());
		auto res = objects.emplace(id, std::make_shared<Object>(id, subtree));
		res.first->second->minExtent = Eigen::Vector3d::Constant(std::numeric_limits<double>::max());
		res.first->second->maxExtent = Eigen::Vector3d::Constant(std::numeric_limits<double>::lowest());
	}

	// Populate the object representation
	for (mps::VoxelRegion::vertices_size_type v = 0; v < voxelRegion->num_vertices(); ++v)
	{
		const auto label = vertexState[v];
		if (label == VoxelRegion::FREE_SPACE) { continue; }
		const auto descriptor = voxelRegion->vertex_at(v);
		const auto coordinate = voxelRegion->coordinate_of(descriptor);

		ObjectIndex objID(label);
		auto& obj = objects.at(objID);
		obj->occupancy->setNodeValue(coordinate.x(), coordinate.y(), coordinate.z(),
		                             std::numeric_limits<float>::max(), true);
		obj->minExtent = obj->minExtent.cwiseMin(coordinate);
		obj->maxExtent = obj->maxExtent.cwiseMax(coordinate);
	}

	// Prune and approximate shape data
	for (auto& pair : objects)
	{
		pair.second->occupancy->updateInnerOccupancy();
		pair.second->approximation = approximateShape(pair.second->occupancy.get());
		assert(pair.second->approximation);
	}

//	segInfo = std::make_shared<SegmentationInfo>();
//	*segInfo = *scene->segInfo; // Shallow copy; deep copy objectness later
//
//	const auto& os = scene->segInfo->objectness_segmentation;
//	segInfo->objectness_segmentation = boost::make_shared<cv_bridge::CvImage>(
//		os->header, os->encoding,
//		cv::Mat(os->image.size(),
//		        os->image.type()));
//
//	const int segStep = 1;
//	segInfo->objectness_segmentation->image = rayCastOccupancy(*this, scene->cameraModel, scene->worldTcamera, scene->roi, segStep);

//	if (scene->scenario->shouldVisualize("particle_segmentation"))
//	{
////		IMSHOW()
//	}
}

ObjectIndex OccupancyData::coordToObject(const Eigen::Vector3d& pt) const
{
	if (!voxelRegion->isInRegion(pt)) { return ObjectIndex(VoxelRegion::FREE_SPACE); }
	return ObjectIndex(vertexState.at(voxelRegion->index_of(voxelRegion->coordToVertexDesc(pt))));
}

cv::Rect2d occupancyToROI(const OccupancyData& state, const image_geometry::PinholeCameraModel& cameraModel, const moveit::Pose& worldTcamera)
{
	Eigen::AlignedBox2d aabb;
	Eigen::AlignedBox2d imageAABB(Eigen::Vector2d{0, 0},
	                              Eigen::Vector2d{cameraModel.cameraInfo().width, cameraModel.cameraInfo().height});
	moveit::Pose cameraTworld = worldTcamera.inverse(Eigen::Isometry);
	for (const auto& obj : state.objects)
	{
		// Get all corners of box
		for (const auto& corner : getCorners(obj.second->minExtent, obj.second->maxExtent, 3))
		{
			Eigen::Vector3d p = cameraTworld * corner;
			cv::Point3d worldPt_camera(p.x(), p.y(), p.z());
			cv::Point2d imgPt = cameraModel.project3dToPixel(worldPt_camera);
			Eigen::Vector2d pt{imgPt.x, imgPt.y};

			aabb.extend(pt);
		}
	}

	aabb = aabb.intersection(imageAABB);

	return {aabb.min().x(), aabb.min().y(), aabb.max().x()-aabb.min().x(), aabb.max().y()-aabb.min().y()};
}

cv::Rect2d workspaceToROI(const VoxelRegion& region, const image_geometry::PinholeCameraModel& cameraModel, const moveit::Pose& worldTcamera)
{
	Eigen::AlignedBox2d aabb;
	Eigen::AlignedBox2d imageAABB(Eigen::Vector2d{0, 0},
	                              Eigen::Vector2d{cameraModel.cameraInfo().width, cameraModel.cameraInfo().height});
	moveit::Pose cameraTworld = worldTcamera.inverse(Eigen::Isometry);

	// Get all corners of box
	for (const auto& corner : getCorners(region.regionMin, region.regionMax, 3))
	{
		Eigen::Vector3d p = cameraTworld * corner;
		cv::Point3d worldPt_camera(p.x(), p.y(), p.z());
		cv::Point2d imgPt = cameraModel.project3dToPixel(worldPt_camera);
		Eigen::Vector2d pt{imgPt.x, imgPt.y};

		aabb.extend(pt);
	}

	aabb = aabb.intersection(imageAABB);

	return {aabb.min().x(), aabb.min().y(), aabb.max().x()-aabb.min().x(), aabb.max().y()-aabb.min().y()};
}

cv::Mat rayCastOccupancy(const OccupancyData& state, const image_geometry::PinholeCameraModel& cameraModel, const moveit::Pose& worldTcamera, const cv::Rect& roi, const int& step)
{
//	auto roi = cameraModel.rectifiedRoi();

	using LabelT = uint16_t;
	using DepthT = float;
	cv::Mat labels = cv::Mat::zeros(cameraModel.cameraInfo().height, cameraModel.cameraInfo().width, CV_16U);
	cv::Mat depthBuf(cameraModel.cameraInfo().height, cameraModel.cameraInfo().width, CV_32F, std::numeric_limits<DepthT>::max());

//	std::map<ObjectIndex, std::shared_ptr<octomap::OcTree>> labelToOcTreeLookup = particle.state->voxelRegion->vertexLabelToOctrees(particle.state->vertexState, particle.state->uniqueObjectLabels);
//	std::cerr << "labelToOcTreeLookup contains " << labelToOcTreeLookup.size() << " elements." << std::endl;

	const Eigen::Vector3f r0_world = worldTcamera.translation().cast<float>();
	const octomap::point3d cameraPose(r0_world.x(), r0_world.y(), r0_world.z());

//	roi.y = 0; roi.height = cameraModel.cameraInfo().height;
//	roi.x = 0; roi.width = cameraModel.cameraInfo().width;

	// TODO: TOO SLOW!!!
#pragma omp parallel for
	for (int v = roi.y; v < roi.y+roi.height; /*++v*/v+=step)
	{
		for (int u = roi.x; u < roi.x + roi.width; /*++u*/u+=step)
		{
			const Eigen::Vector3f rn_world = (worldTcamera.linear() * toPoint3D<Eigen::Vector3d>(u, v, 0.5, cameraModel).normalized()).cast<float>();
			const octomap::point3d rayPoint(rn_world.x(), rn_world.y(), rn_world.z());

			const Eigen::Vector3f rnInv_world = rn_world.cast<float>().cwiseInverse();

			for (const auto& pair : state.objects)
			{
				// Check AABB intersection
				const auto& obj = pair.second; // particle.state->objects.at(pair.first);
				bool mightHit = fast_ray_box_intersection(r0_world, rnInv_world, obj->minExtent.cast<float>(), obj->maxExtent.cast<float>());
				if (!mightHit) { continue; }

				octomap::point3d intsectPoint;
				if (obj->occupancy->castRay(cameraPose, rayPoint, intsectPoint, true, 2.0))
				{
//					std::cerr << "Intersection with octree " << pair.first << std::endl;
					double dist = (intsectPoint - cameraPose).norm();
					auto& zBuf = depthBuf.at<DepthT>(v, u);
					if (dist < zBuf)
					{
						zBuf = dist;
						labels.at<LabelT>(v, u) = pair.first.id; //// voxel label == 0: free sapce; seg label == 0: free space;
//						std::cerr << "label value: " << labels.at<LabelT>(v, u) << std::endl;
					}
				}
			}
		}
	}
	return labels;
}

}


/*
Boost Software License - Version 1.0 - August 17th, 2003

Permission is hereby granted, free of charge, to any person or organization
obtaining a copy of the software and accompanying documentation covered by
this license (the "Software") to use, reproduce, display, distribute,
execute, and transmit the Software, and to prepare derivative works of the
Software, and to permit third-parties to whom the Software is furnished to
do so, all subject to the following:

The copyright notices in the Software and this entire statement, including
the above license grant, this restriction and the following disclaimer,
must be included in all copies of the Software, in whole or in part, and
all derivative works of the Software, unless such copies or derivative
works are solely in the form of machine-executable object code generated by
a source language processor.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.

					License Agreement
					For Open Source Computer Vision Library
					(3-clause BSD License)
 Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:

   * Redistribution's of source code must retain the above copyright notice,
     this list of conditions and the following disclaimer.

   * Redistribution's in binary form must reproduce the above copyright notice,
     this list of conditions and the following disclaimer in the documentation
     and/or other materials provided with the distribution.

   * The name of the copyright holders may not be used to endorse or promote products
     derived from this software without specific prior written permission.

 This software is provided by the copyright holders and contributors "as is" and
 any express or implied warranties, including, but not limited to, the implied
 warranties of merchantability and fitness for a particular purpose are disclaimed.
 In no event shall the Intel Corporation or contributors be liable for any direct,
 indirect, incidental, special, exemplary, or consequential damages
 (including, but not limited to, procurement of substitute goods or services;
 loss of use, data, or profits; or business interruption) however caused
 and on any theory of liability, whether in contract, strict liability,
 or tort (including negligence or otherwise) arising in any way out of
 the use of this software, even if advised of the possibility of such damage.
     Author: Trent Weiss
 */

#include "voronoi_art/voronoi_processing/voronoi_processing.h"
#include "voronoi_art/image_processing/image_processing.h"
#include <math.h>
#include <boost/bind.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <iostream>
#include <boost/timer/timer.hpp>
#include <Eigen/Dense>
namespace voronoi_art {
struct point_slope {
	Eigen::Vector2d x;
	double y;
};
typedef struct point_slope point_slope_t;
voronoi_processing::voronoi_processing(const Mat& input_image,
		const std::vector<point_type>& site_points, bool extract_delaunay) {
	vd_.reset(new VD);
	input_image_ = input_image.clone();
	input_image_pixels_ = image_processing::image_to_point_vector(input_image_);
	site_points_ = std::vector<point_type>(site_points);
	{
		boost::timer::auto_cpu_timer voronoi_timer("Voronoi Diagram took: %t sec CPU, %w sec real\n");
		construct_voronoi(site_points_.begin(), site_points_.end(), vd_.get());
	}
	if (extract_delaunay) {
		delaunay_triangulation_.reset(new delaunay_triangulation);
		boost::timer::auto_cpu_timer voronoi_timer("Extracting Delaunay Triangulation took: %t sec CPU, %w sec real\n");
		extract_delaunay_triangulation();
	}
}
voronoi_processing::~voronoi_processing() {
	// TODO Auto-generated destructor stub
}
//Eigen::Vector2d po
void voronoi_processing::add_delaunay_half_segments(
		VD::const_cell_iterator& it) {
	point_type site_point = point_type(site_points_[it->source_index()]);
	voronoi_processing::delaunay_vertex u = boost::add_vertex<
			voronoi_processing::delaunay_triangulation>(
			*delaunay_triangulation_);
	(*delaunay_triangulation_)[u] = site_point;
	const voronoi_edge<VD::coordinate_type>* edge = it->incident_edge();
	do {
		edge = edge->next();
		const voronoi_vertex<VD::coordinate_type>* v0 = edge->vertex0();
		const voronoi_vertex<VD::coordinate_type>* v1 = edge->vertex1();
		if (v0 and v1) {
			point_type p0(v0->x(), (v0->y()));
			point_type p1(v1->x(), (v1->y()));
			point_type intersection;
			if (p1.x() == p0.x()) {
				//edge is a vertical line segment.
			     intersection = point_type(p1.x(), site_point.y());
			} else if (p1.y() == p0.y()) {
				//edge is a horizontal line segment.
			     intersection = point_type(site_point.x(), p1.y());
			} else {
				/*
				 * intersection is the solution to a system of linear equations
				 * y-y_p = m * (x - x_p)
				 * y-y_p = m*x - m*x_p
				 * -m*x + y = y_p - m*x_p
				 *
				 */
				Eigen::Vector2d X, Y;
				Eigen::Matrix2d A = Eigen::Matrix2d::Zero();
				double m_edge = (p1.y() - p0.y()) / (p1.x() - p0.x());
				double m_line = -1.0 / m_edge;
				A(0, 0) = -m_edge;
				A(0, 1) = 1.0;
				Y[0] = p0.y() - m_edge * p0.x();

				A(1, 0) = -m_line;
				A(1, 1) = 1.0;
				Y[1] = site_point.y() - m_line * site_point.x();
				X = A.inverse() * Y;
				intersection = point_type(X.x(), X.y());
			}
			voronoi_processing::delaunay_vertex v = boost::add_vertex<
					voronoi_processing::delaunay_triangulation>(
					*delaunay_triangulation_);
			(*delaunay_triangulation_)[v] = intersection;

			delaunay_edge e;
			bool b;
			boost::tie(e, b) = boost::add_edge<
					voronoi_processing::delaunay_triangulation>(u, v,
					*delaunay_triangulation_);
			(*delaunay_triangulation_)[e] = input_image_.at<Vec3b>(
					cv_float_point(site_point.x(), site_point.y()));
		}
	} while (edge != it->incident_edge());
}
void voronoi_processing::extract_delaunay_triangulation() {

	for (VD::const_cell_iterator it = vd_->cells().begin();
			it != vd_->cells().end(); ++it) {

		if ((not it->is_degenerate())) {
			add_delaunay_half_segments(it);
		}
	}
}
point_type voronoi_processing::cv_point_to_voronoi(const cv_float_point& pt) {
	return point_type(pt.x, pt.y);
}

cv_float_point voronoi_processing::voronoi_vertex_to_cv_point(
		const VD::vertex_type* vertex) {
	float x = (float) vertex->x();
	float y = (float) vertex->y();
	return cv_float_point(x, y);
}

std::shared_ptr<const voronoi_art::VD> voronoi_processing::get_voronoi_diagram() const {
	return vd_;
}

const std::vector<point_type>& voronoi_processing::get_site_points() const {
	return site_points_;
}
inline double euclidean_distance(const point_type& a, const point_type& b) {
	return std::sqrt(std::pow(a.x() - b.x(), 2) + std::pow(a.y() - b.y(), 2));
}
bool sort_functor(const point_type& a, const point_type& b,
		const point_type& reference_point) {
	return euclidean_distance(a, reference_point)
			< euclidean_distance(b, reference_point);
}
vector<cv_int_point> voronoi_processing::voronoi_cell_to_cv_int_polygon(
		const VD::cell_type cell) {

	vector<cv_int_point> rtn;
	const voronoi_edge<VD::coordinate_type>* edge = cell.incident_edge();
	do {
		edge = edge->next();
		const voronoi_vertex<VD::coordinate_type>* v0 = edge->vertex0();
		if (!v0) {
			continue;
		}
		cv_float_point pt = voronoi_vertex_to_cv_point(edge->vertex0());
		rtn.push_back(cv_int_point(std::round(pt.x), std::round(pt.y)));
	} while (edge != cell.incident_edge());
	return rtn;
}
vector<cv_float_point> voronoi_processing::voronoi_cell_to_cv_float_polygon(
		const VD::cell_type cell) {

	vector<cv_float_point> rtn;
	const voronoi_edge<double>* edge = cell.incident_edge();
	do {
		edge = edge->next();
		const voronoi_vertex<double>* v0 = edge->vertex0();
		if (!v0) {
			continue;
		}
		rtn.push_back(voronoi_vertex_to_cv_point(edge->vertex0()));
	} while (edge != cell.incident_edge());
	return rtn;
}
Scalar average_color(const vector<Scalar>& colors) {
	unsigned long r = 0, g = 0, b = 0;
	for (const Scalar& color : colors) {
		r += color[0];
		g += color[1];
		b += color[2];
	}
	float size = (float) colors.size();
	r = std::round((float) r / size);
	g = std::round((float) g / size);
	b = std::round((float) b / size);
	return Scalar(r, g, b, 1.0);
}
vector<cv_int_point> extractIntPoints(const vector<Pixel>& pixels) {
	vector<cv_int_point> rtn(pixels.size());
	for (unsigned int i = 0; i < rtn.size(); ++i) {
		rtn[i] = pixels[i].get_int_point();
	}
	return rtn;
}

void voronoi_processing::draw_cell(voronoi_art::VD::const_cell_iterator& cell,
		vector<Pixel>& pixels_copy, Mat& image) const {
	vector<cv_int_point> cv_poly_int = voronoi_cell_to_cv_int_polygon(*cell);
	point_type pt = site_points_[cell->source_index()];
	Mat mask(image.size(),CV_8U, Scalar::all(0.0));
	cv::fillConvexPoly(mask, &cv_poly_int[0], cv_poly_int.size(), Scalar(255.0,0.0,0.0,1.0));
	Scalar color = cv::mean(input_image_,mask);

	cv::fillConvexPoly(image, &cv_poly_int[0], cv_poly_int.size(), color);

}
void voronoi_processing::draw_cells(Mat& image) const {

	vector<Pixel> pixels_copy(input_image_pixels_);
	for (VD::const_cell_iterator it = vd_->cells().begin();
			it != vd_->cells().end(); ++it) {

		if (it->contains_point() and (not it->is_degenerate())) {

			draw_cell(it, pixels_copy, image);
		}
	}
}

void voronoi_processing::draw_edge(const voronoi_art::VD::edge_type& edge,
		Mat& image) const {

	const VD::vertex_type* v0 = edge.vertex0();
	Scalar color;
	if (v0 and edge.next()->vertex0()) {
		int r = std::round(site_points_[edge.cell()->source_index()].y());
		int c = std::round(site_points_[edge.cell()->source_index()].x());
		color = input_image_.at<Vec3b>(r, c);

		line(image, voronoi_vertex_to_cv_point(v0),
				voronoi_vertex_to_cv_point(edge.next()->vertex0()), color);
	}

}
void voronoi_processing::draw_edges(Mat& image) const {
	for (VD::const_edge_iterator it = vd_->edges().begin();
			it != vd_->edges().end(); ++it) {

		if (it->is_primary() and (not it->is_infinite())) {
			draw_edge(*it, image);
		}
	}
}

void voronoi_processing::draw_delaunay_edges(Mat& image) const {

	std::pair<boost::graph_traits<delaunay_triangulation>::edge_iterator,
			boost::graph_traits<delaunay_triangulation>::edge_iterator> range =
			boost::edges(*delaunay_triangulation_);
	boost::graph_traits<delaunay_triangulation>::edge_iterator it = range.first;
	boost::graph_traits<delaunay_triangulation>::edge_iterator end =
			range.second;
	while (it != end) {
		point_type a = (*delaunay_triangulation_)[it->m_source];
		point_type b = (*delaunay_triangulation_)[it->m_target];

		cv_float_point acv(a.x(), a.y());
		cv_float_point bcv(b.x(), b.y());
		Scalar* color = (Scalar*) (it->m_eproperty);
		line(image, acv, bcv, *color);
		it++;
	}
}
voronoi_processing::delaunay_triangulation_ptr voronoi_processing::get_delaunay_triangulation() const {
	return delaunay_triangulation_ptr(delaunay_triangulation_);
}

void voronoi_processing::draw_delaunay_cells(Mat& image) const {
}
} /* namespace voronoi_art */


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"
#include "graph_python_interface.hh"
#include "numpy_bind.hh"
#include "hash_map_wrap.hh"
#include "coroutine.hh"
#include "graph_python_interface.hh"

#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python.hpp>

#if (BOOST_VERSION >= 106000)
# include <boost/math/special_functions/relative_difference.hpp>
#endif

using namespace std;
using namespace boost;
using namespace graph_tool;

struct stop_search {};

template <class DistMap, class PredMap>
class bfs_max_visitor:
    public boost::bfs_visitor<null_visitor>
{
public:
    bfs_max_visitor(DistMap dist_map, PredMap pred, size_t max_dist,
                    size_t source, size_t target, std::vector<size_t>& reached)
        : _dist_map(dist_map), _pred(pred), _max_dist(max_dist),
          _source(source), _target(target), _dist(0), _reached(reached) {}

    ~bfs_max_visitor()
    {
        typedef typename property_traits<DistMap>::value_type dist_t;
        constexpr dist_t inf = std::is_floating_point<dist_t>::value ?
            numeric_limits<dist_t>::infinity() :
            numeric_limits<dist_t>::max();
        for (auto v : _unreached)
            _dist_map[v] = inf;
    }

    template <class Graph>
    void tree_edge(typename graph_traits<Graph>::edge_descriptor e,
                   Graph& g)
    {
        _pred[target(e,g)] = source(e,g);
    }

    template <class Graph>
    void examine_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                        Graph&)
    {
        if ( _dist_map[v] > _max_dist)
            throw stop_search();
    }

    template <class Graph>
    void discover_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                         Graph&)
    {
        auto p = _pred[v];
        if (size_t(p) == v)
            return;
        _dist_map[v] = _dist_map[p] + 1;
        _reached.push_back(v);
        if (_dist_map[v] > _max_dist)
            _unreached.push_back(v);
        if (v == _target)
            throw stop_search();
    }

private:
    DistMap _dist_map;
    PredMap _pred;
    typename property_traits<DistMap>::value_type _max_dist;
    size_t _source;
    size_t _target;
    size_t _dist;
    std::vector<size_t> _unreached;
    std::vector<size_t>& _reached;
};

template <class DistMap, class PredMap>
class bfs_max_multiple_targets_visitor:
    public boost::bfs_visitor<null_visitor>
{
public:
    bfs_max_multiple_targets_visitor(DistMap dist_map, PredMap pred,
                                     size_t max_dist, size_t source,
                                     gt_hash_set<std::size_t> target,
                                     std::vector<size_t>& reached)
        : _dist_map(dist_map), _pred(pred), _max_dist(max_dist),
          _source(source), _target(target), _dist(0), _reached(reached) {}

    ~bfs_max_multiple_targets_visitor()
    {
        typedef typename property_traits<DistMap>::value_type dist_t;
        constexpr dist_t inf = std::is_floating_point<dist_t>::value ?
            numeric_limits<dist_t>::infinity() :
            numeric_limits<dist_t>::max();
        for (auto v : _unreached)
            _dist_map[v] = inf;
    }

    template <class Graph>
    void tree_edge(typename graph_traits<Graph>::edge_descriptor e,
                   Graph& g)
    {
        _pred[target(e,g)] = source(e,g);
    }

    template <class Graph>
    void examine_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                        Graph&)
    {
        if ( _dist_map[v] > _max_dist)
            throw stop_search();
    }

    template <class Graph>
    void discover_vertex(typename graph_traits<Graph>::vertex_descriptor v,
                         Graph&)
    {
        auto p = _pred[v];
        if (size_t(p) == v)
            return;
        _dist_map[v] = _dist_map[p] + 1;

        if (_dist_map[v] > _max_dist)
            _unreached.push_back(v);

        auto iter = _target.find(v);
        if (iter != _target.end())
        {
            _target.erase(iter);
            if (_target.empty())
                throw stop_search();
        };
    }

private:
    DistMap _dist_map;
    PredMap _pred;
    typename property_traits<DistMap>::value_type _max_dist;
    size_t _source;
    gt_hash_set<std::size_t> _target;
    size_t _dist;
    std::vector<size_t> _unreached;
    std::vector<size_t>& _reached;
};

template <class DistMap>
class djk_max_visitor:
    public boost::dijkstra_visitor<null_visitor>
{
public:
    djk_max_visitor(DistMap dist_map,
                    typename property_traits<DistMap>::value_type max_dist,
                    typename property_traits<DistMap>::value_type inf,
                    size_t target, std::vector<size_t>& reached)
        : _dist_map(dist_map), _max_dist(max_dist), _inf(inf),
          _target(target), _reached(reached) {}

    ~djk_max_visitor()
    {
        for (auto v : _unreached)
            _dist_map[v] = _inf;
    }

    template <class Graph>
    void examine_vertex(typename graph_traits<Graph>::vertex_descriptor u,
                        Graph&)
    {
        if (_dist_map[u] > _max_dist)
            throw stop_search();

        if (u == _target)
            throw stop_search();
    }

    template <class Graph>
    void discover_vertex(typename graph_traits<Graph>::vertex_descriptor u,
                         Graph&)
    {
        if (_dist_map[u] > _max_dist)
            _unreached.push_back(u);
        _reached.push_back(u);
    }

private:
    DistMap _dist_map;
    typename property_traits<DistMap>::value_type _max_dist;
    typename property_traits<DistMap>::value_type _inf;
    size_t _target;
    std::vector<size_t> _unreached;
    std::vector<size_t>& _reached;
};

template <class DistMap>
class djk_max_multiple_targets_visitor:
    public boost::dijkstra_visitor<null_visitor>
{
public:
    djk_max_multiple_targets_visitor(DistMap dist_map,
                                     typename property_traits<DistMap>::value_type max_dist,
                                     typename property_traits<DistMap>::value_type inf,
                                     gt_hash_set<std::size_t> target,
                                     std::vector<size_t>& reached)
        : _dist_map(dist_map), _max_dist(max_dist), _inf(inf),
          _target(target), _reached(reached) {}

    ~djk_max_multiple_targets_visitor()
    {
        for (auto v : _unreached)
            _dist_map[v] = _inf;
    }

    template <class Graph>
    void examine_vertex(typename graph_traits<Graph>::vertex_descriptor u,
                        Graph&)
    {
        if (_dist_map[u] > _max_dist)
            throw stop_search();

        auto iter = _target.find(u);
        if (iter != _target.end())
        {
            _target.erase(iter);
            if (_target.empty())
                throw stop_search();
        };
    }

    template <class Graph>
    void discover_vertex(typename graph_traits<Graph>::vertex_descriptor u,
                         Graph&)
    {
        if (_dist_map[u] > _max_dist)
            _unreached.push_back(u);
        _reached.push_back(u);
    }

private:
    DistMap _dist_map;
    typename property_traits<DistMap>::value_type _max_dist;
    typename property_traits<DistMap>::value_type _inf;
    gt_hash_set<std::size_t> _target;
    std::vector<size_t> _unreached;
    std::vector<size_t>& _reached;
};

struct do_bfs_search
{
    template <class Graph, class VertexIndexMap, class DistMap, class PredMap>
    void operator()(const Graph& g, size_t source,
                    boost::python::object otarget_list,
                    VertexIndexMap vertex_index, DistMap dist_map,
                    PredMap pred_map, long double max_dist,
                    std::vector<size_t>& reached) const
    {
        typedef typename property_traits<DistMap>::value_type dist_t;

        auto target_list = get_array<int64_t, 1>(otarget_list);
        gt_hash_set<std::size_t> tgt(target_list.begin(),
                                     target_list.end());

        dist_t inf = std::is_floating_point<dist_t>::value ?
            numeric_limits<dist_t>::infinity() :
            numeric_limits<dist_t>::max();

        dist_t max_d = (max_dist > 0) ? max_dist : inf;

        dist_map[source] = 0;

        unchecked_vector_property_map<boost::default_color_type, VertexIndexMap>
        color_map(vertex_index, num_vertices(g));
        try
        {
            if (tgt.size() <= 1)
            {
                size_t target = tgt.empty() ?
                    graph_traits<GraphInterface::multigraph_t>::null_vertex() :
                    *tgt.begin();
                breadth_first_visit(g, vertex(source, g),
                                    visitor(bfs_max_visitor<DistMap, PredMap>
                                            (dist_map, pred_map, max_d,
                                             source, target, reached)).
                                    vertex_index_map(vertex_index).
                                    color_map(color_map));
            }
            else
            {
                breadth_first_visit(g, vertex(source, g),
                                    visitor(bfs_max_multiple_targets_visitor<DistMap, PredMap>
                                            (dist_map, pred_map, max_d,
                                             source, tgt, reached)).
                                    vertex_index_map(vertex_index).
                                    color_map(color_map));
            }

        }
        catch (stop_search&) {}
    }
};

struct do_djk_search
{
    template <class Graph, class VertexIndexMap, class DistMap, class PredMap,
              class WeightMap>
    void operator()(const Graph& g, size_t source,
                    boost::python::object otarget_list,
                    VertexIndexMap vertex_index, DistMap dist_map,
                    PredMap pred_map, WeightMap weight, long double max_dist,
                    std::vector<size_t>& reached) const
    {
        auto target_list = get_array<int64_t, 1>(otarget_list);

        typedef typename property_traits<DistMap>::value_type dist_t;

        constexpr dist_t inf = (std::is_floating_point<dist_t>::value) ?
            numeric_limits<dist_t>::infinity() :
            numeric_limits<dist_t>::max();

        dist_t max_d = (max_dist > 0) ? max_dist : inf;

        gt_hash_set<std::size_t> tgt(target_list.begin(),
                                     target_list.end());

        dist_map[source] = 0;

        try
        {
            if (tgt.size() <= 1)
            {
                size_t target = tgt.empty() ?
                    graph_traits<GraphInterface::multigraph_t>::null_vertex() :
                    *tgt.begin();
                dijkstra_shortest_paths_no_color_map_no_init
                    (g, vertex(source, g), pred_map, dist_map, weight,
                     vertex_index, std::less<dist_t>(),
                     boost::closed_plus<dist_t>(), inf, dist_t(),
                     djk_max_visitor<DistMap>(dist_map, max_d, inf, target,
                                              reached));
            }
            else
            {
                dijkstra_shortest_paths_no_color_map_no_init
                    (g, vertex(source, g), pred_map, dist_map, weight,
                     vertex_index, std::less<dist_t>(),
                     boost::closed_plus<dist_t>(), inf, dist_t(),
                     djk_max_multiple_targets_visitor<DistMap>(dist_map, max_d,
                                                               inf, tgt,
                                                               reached));
            }

        }
        catch (stop_search&) {}

    }
};

struct do_bf_search
{
    template <class Graph, class DistMap, class PredMap, class WeightMap>
    void operator()(const Graph& g, size_t source, DistMap dist_map,
                    PredMap pred_map, WeightMap weight) const
    {
        bool ret = bellman_ford_shortest_paths(g, root_vertex(source).
                                               predecessor_map(pred_map).
                                               distance_map(dist_map).
                                               weight_map(weight));
        if (!ret)
            throw ValueException("Graph contains negative loops");

        // consistency with dijkstra
        typedef typename property_traits<DistMap>::value_type dist_t;
        if (std::is_floating_point<dist_t>::value)
        {
            for (auto v : vertices_range(g))
            {
                if (dist_map[v] == numeric_limits<dist_t>::max())
                    dist_map[v] = numeric_limits<dist_t>::infinity();
            }
        }
    }
};

void get_dists(GraphInterface& gi, size_t source, boost::python::object tgt,
               boost::any dist_map, boost::any weight, boost::any pred_map,
               long double max_dist, bool bf, std::vector<size_t>& reached)
{
    typedef property_map_type
        ::apply<int64_t, GraphInterface::vertex_index_map_t>::type pred_map_t;

    pred_map_t pmap = any_cast<pred_map_t>(pred_map);

    if (weight.empty())
    {
        run_action<>()
            (gi, std::bind(do_bfs_search(), std::placeholders::_1, source, tgt, gi.get_vertex_index(),
                           std::placeholders::_2, pmap.get_unchecked(num_vertices(gi.get_graph())),
                           max_dist, std::ref(reached)),
             writable_vertex_scalar_properties())
            (dist_map);
    }
    else
    {
        if (bf)
        {
            run_action<>()
                (gi, std::bind(do_bf_search(), std::placeholders::_1, source,
                               std::placeholders::_2, pmap.get_unchecked(num_vertices(gi.get_graph())),
                               std::placeholders::_3),
                 writable_vertex_scalar_properties(),
                 edge_scalar_properties())
                (dist_map, weight);
        }
        else
        {
            run_action<>()
                (gi, std::bind(do_djk_search(), std::placeholders::_1, source, tgt, gi.get_vertex_index(),
                               std::placeholders::_2, pmap.get_unchecked(num_vertices(gi.get_graph())),
                               std::placeholders::_3, max_dist, std::ref(reached)),
                 writable_vertex_scalar_properties(),
                 edge_scalar_properties())
                (dist_map, weight);
        }
    }
}

template <class Graph, class Dist, class Pred, class Weight, class Preds>
void get_all_preds(Graph g, Dist dist, Pred pred, Weight weight, Preds preds,
                   long double epsilon)
{
    parallel_vertex_loop
        (g,
         [&](auto v)
         {
            if (size_t(pred[v]) == v)
                return;
            auto d = dist[v];
            typedef decltype(d) dist_t;
            for (auto e : in_or_out_edges_range(v, g))
            {
                auto u = source(e, g);
                if (!std::is_floating_point<dist_t>::value)
                {
                    if (dist_t(dist[u] + get(weight, e)) == d)
                        preds[v].push_back(u);
                }
                else
                {
#if (BOOST_VERSION >= 106000)
                    if (boost::math::relative_difference(dist_t(dist[u] + get(weight, e)), d) < epsilon)
                        preds[v].push_back(u);
#else
                    if (abs(dist_t(dist[u] + get(weight, e)) - d) < epsilon)
                        preds[v].push_back(u);
#endif
                }
            }
         });
};

void do_get_all_preds(GraphInterface& gi, boost::any adist, boost::any apred,
                      boost::any aweight, boost::any apreds,
                      long double epsilon)
{
    typedef property_map_type
        ::apply<int64_t, GraphInterface::vertex_index_map_t>::type pred_map_t;
    typedef property_map_type
        ::apply<vector<int64_t>, GraphInterface::vertex_index_map_t>::type preds_map_t;

    pred_map_t pred = any_cast<pred_map_t>(apred);
    preds_map_t preds = any_cast<preds_map_t>(apreds);

    typedef UnityPropertyMap<int,GraphInterface::edge_t> weight_map_t;
    typedef boost::mpl::push_back<edge_scalar_properties, weight_map_t>::type
        weight_props_t;

    if (aweight.empty())
        aweight = weight_map_t();

    run_action<>()
        (gi, [&](auto& g, auto dist, auto weight)
             {get_all_preds(g, dist, pred.get_unchecked(num_vertices(g)),
                            weight, preds.get_unchecked(num_vertices(g)),
                            epsilon);},
         vertex_scalar_properties(), weight_props_t())(adist, aweight);
}

template <class Pred, class Yield>
void get_all_shortest_paths(size_t s, size_t t, Pred pred, Yield& yield)
{
    vector<size_t> path;
    vector<pair<size_t, size_t>> stack = {{t, 0}};
    while (!stack.empty())
    {
        size_t v, i;
        std::tie(v, i) = stack.back();
        if (v == s)
        {
            path.clear();
            for (auto iter = stack.rbegin(); iter != stack.rend(); ++iter)
                path.push_back(iter->first);
            yield(wrap_vector_owned<size_t>(path));
        }
        if (pred[v].size() > i)
        {
            stack.emplace_back(pred[v][i], 0);
        }
        else
        {
            stack.pop_back();
            if (!stack.empty())
                ++stack.back().second;
        }
    }
};

python::object do_get_all_shortest_paths(GraphInterface& gi, size_t s, size_t t,
                                         boost::any apred)
{
#ifdef HAVE_BOOST_COROUTINE
    auto dispatch = [&](auto& yield)
        {
            run_action<>()
                (gi, [&](auto&, auto pred)
                     {get_all_shortest_paths(s, t, pred, yield);},
                 vertex_scalar_vector_properties())(apred);
        };
    return python::object(CoroGenerator(dispatch));
#else
    throw GraphException("This functionality is not available because boost::coroutine was not found at compile-time");
#endif // HAVE_BOOST_COROUTINE
}

template <class Graph, class Yield, class VMap>
void get_all_paths(size_t s, size_t t, size_t cutoff, VMap visited,
                   Yield& yield, Graph& g)
{
    typedef typename graph_traits<Graph>::out_edge_iterator eiter_t;
    typedef std::pair<eiter_t, eiter_t> item_t;

    visited[s] = true;
    vector<size_t> vs = {s};
    vector<item_t> stack = {out_edges(s, g)};
    while (!stack.empty())
    {
        auto& pos = stack.back();
        if (pos.first == pos.second || stack.size() > cutoff)
        {
            visited[vs.back()] = false;
            vs.pop_back();
            stack.pop_back();
            if (!stack.empty())
                ++stack.back().first;
            continue;
        }

        auto v = target(*pos.first, g);

        if (v == t)
        {
            vector<size_t> path = {s};
            for (auto& ei : stack)
                path.push_back(target(*ei.first, g));

            yield(wrap_vector_owned<size_t>(path));

            ++pos.first;
        }
        else
        {
            if (!visited[v])
            {
                visited[v] = true;
                vs.push_back(v);
                stack.push_back(out_edges(v, g));
            }
            else
            {
                ++pos.first;
            }
        }
    }
};

python::object do_get_all_paths(GraphInterface& gi, size_t s, size_t t,
                                size_t cutoff, boost::any avisited)
{
#ifdef HAVE_BOOST_COROUTINE
    typedef vprop_map_t<uint8_t>::type vprop_t;
    vprop_t visited = boost::any_cast<vprop_t>(avisited);
    auto dispatch = [&](auto& yield)
        {
            run_action<>()
            (gi, [&](auto& g) {get_all_paths(s, t, cutoff,
                                             visited.get_unchecked(), yield,
                                             g);})();
        };
    return python::object(CoroGenerator(dispatch));
#else
    throw GraphException("This functionality is not available because boost::coroutine was not found at compile-time");
#endif // HAVE_BOOST_COROUTINE
}

void export_dists()
{
    python::def("get_dists", &get_dists);
    python::def("get_all_preds", &do_get_all_preds);
    python::def("get_all_shortest_paths", &do_get_all_shortest_paths);
    python::def("get_all_paths", &do_get_all_paths);
};