#include "Animation.h"
#include "ModelCache.h"
#include "Path.h"
#include "Profiler.h"
#include "ProgramOptions.h"
#include "Projection.h"
#include "ProjectMesh.h"
#include "RateIndicator.h"
#include "RenderMesh.h"
#include "Script.h"

#include <Eigen/Dense>

#include <algorithm>
#include <chrono>
#include <fstream>
#include <future>
#include <iomanip>
#include <iostream>
#include <locale>
#include <mutex>
#include <numeric>
#include <boost/program_options.hpp>
#include <sstream>
#include <string>
#include <vector>

using namespace std;
using namespace Eigen;

namespace
{
	void PrintProfilerNode(ostream & stream, size_t level, Profiler::Node * node)
	{
		size_t nameLength (0u);
		double total      (0.0);
		for (const auto & timerStat : node->children)
		{
			nameLength = max(nameLength, timerStat.first.size());
			total += timerStat.second->stats.Total();
		}

		for (const auto & timerStat : node->children)
		{
			const string     & name = timerStat.first;
			const Profiler::Stats & stat = timerStat.second->stats;

			for (size_t i(0); i != level; ++i)
				stream << "  ";

			string pad(nameLength + 1 - name.size(), ' ');
			stream << name << ':' << pad;

			stream << setprecision(6) << stat.Mean();
			if (stat.Count() > 1)
				stream << "(" << setprecision(6) << stat.StDev() << ")";
			stream << "s";
			if (stat.Count() > 1)
				stream << " x" << stat.Count();
			if (node->children.size() > 1)
				stream << " " << setprecision(2) << (100.0 * stat.Total() / total) << '%';

			stream << '\n';

			PrintProfilerNode(stream, level + 1, timerStat.second.get());
		}
	}

	void PrintFrameInfo(size_t frameIndex, size_t frameCount, const Profiler & profiler)
	{
		ostringstream msg;
		msg.setf(ios::fixed, ios::floatfield);

		msg << "frame " << frameIndex << " out of " << frameCount << '\n';

		PrintProfilerNode(msg, 1, profiler.current);

		cout << msg.str() << flush;
	}

	struct FrameSetPrinter : boost::static_visitor<ostream &>
	{
		ostream & stream;

		explicit FrameSetPrinter(ostream & stream) : stream(stream) {}

		ostream & operator() (FramesAll)    { return stream << "all frames"; }
		ostream & operator() (FrameRange r) { return stream << "frames " << get<0>(r) << " to " << get<1>(r); }
		ostream & operator() (FrameIndex i) { return stream << "frame " << i; }
	};

	ostream & operator << (ostream & stream, const FrameSet & frameSet)
	{
		FrameSetPrinter printer(stream);
		return boost::apply_visitor(printer, frameSet);
	}

	void PrintScript(const Script & script)
	{
		cout
			<< "Rendering '" << script.model << "' to '" << script.outputPath << "': "
			<< script.duration << "s at "
			<< script.fps << " fps, "
			<< to_string(script.res.w) << "x" << to_string(script.res.h) << ", "
			<< script.aamask.size() << "x AA, "
			<< script.frames << "\n"
			<< '\n';
	}

	struct FrameExpander : boost::static_visitor<vector<size_t>>
	{
		size_t frameCount;

		explicit FrameExpander(size_t frameCount) : frameCount(frameCount) {}

		vector<size_t> operator() (FramesAll) const
		{
			return MakeRange(0, frameCount);
		}

		vector<size_t> operator() (FrameRange r) const
		{
			size_t min = get<0>(r);
			size_t max = get<1>(r);
			return (min < frameCount)
				? MakeRange(min, std::min(max + 1, frameCount))
				: vector<size_t>();
		}

		vector<size_t> operator() (FrameIndex i) const
		{
			return { i };
		}

		vector<size_t> MakeRange(size_t min, size_t max) const
		{
			vector<size_t> frames(max - min);
			frames.reserve(frameCount);
			iota(frames.rbegin(), frames.rend(), min);
			return frames;
		}
	};

	void Run(const Path & projectRoot, const Script & script, int thread_count, Profiler & globalProfiler)
	{
		Profiler::Timer timer(globalProfiler, "total time");

		const Resolution & res(script.res);

		const Mesh mesh(LoadPly(projectRoot / script.meshPath));

		// set up the camera
		const float    focalDistance = 1.0f;
		const Matrix3f rayCast       = RayCast(res, focalDistance);
		const Matrix4f projection    = Perspective(focalDistance);

		// create an array of frame indices
		const size_t   frameCount = static_cast<size_t>(script.duration * script.fps + 0.5f);
		vector<size_t> frames     = boost::apply_visitor(FrameExpander(frameCount), script.frames);

		// about 0.02 for 1080p
		const float stepLength = 50.0f / (float)sqrt(res.w * res.w + res.h * res.h);

		// set up the model
		ModelCache modelCache;

		mutex frameMutex;

		RateIndicator rateIndicator(60.0);

		auto ProcessFrame = [&]
		{
			Animation animation(script.duration, script.rotationDuration, modelCache, projectRoot / script.model.c_str());

			vector<Vector4f> buffer(res.w * res.h);

			for (;;)
			{
				size_t frame;
				{
					lock_guard<mutex> guard(frameMutex);
					if (frames.empty())
						break;
					frame = frames.back();
					frames.pop_back();
				}

				Profiler profiler;

				{
					Profiler::Timer timer(profiler, "total");

					animation.SetTime(script.duration * frame / frameCount);

					fill(buffer.begin(), buffer.end(), Vector4f::Zero());

					// render
					ProjectMesh(animation.GetCamera(), projection, res, buffer.data(), mesh);
					//SaveProjectionBuffer(projectRoot / "projection.png", res, buffer.data());
					RenderMesh
						( animation.GetCamera(), rayCast, res, buffer.data(), mesh
						, animation.GetModel(), script.aamask, stepLength, profiler, rateIndicator
						);

					// post-process
					AddNoise(res, buffer.data(), script.noise);

					// save
					SaveBuffer
						( projectRoot / script.outputPath / to_string(frame) + ".png"
						, res
						, buffer.data()
						, script.background
						);
				}

				rateIndicator.Reset();
				if (script.printFrameInfo)
					PrintFrameInfo(frame, frameCount, profiler);
			}
		};

		vector<future<void>> threads(thread_count);
		for (auto & t : threads)
			t = async(launch::async, ProcessFrame);
		for (auto & t : threads)
			t.get();
	}
}

int main(int argc, char ** argv)
{
	std::ignore = cout.imbue(locale(""));

	Eigen::initParallel();

	try
	{
		ProgramOptions options(argc, argv);

		Profiler profiler;

		Script script = LoadScript(options.ProjectRoot() / "script.txt");
		PrintScript(script);

		constexpr int thread_count = 10;
		Run(options.ProjectRoot(), script, thread_count, profiler);

		std::cout << "\n";
		PrintProfilerNode(std::cout, 0, profiler.current);
	}
	catch (const exception & e)
	{
		cout << "error: " << e.what() << "\n";
	}

	return 0;
}


///////////////////////////////////////////////////////////////////////////////
// Copyright Christopher Kormanyos 2014.
// Copyright John Maddock 2014.
// Copyright Paul Bristow 2014.
// 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)
//

// Implement quadruple-precision std::numeric_limits<> support.

#ifndef _BOOST_CSTDFLOAT_LIMITS_2014_01_09_HPP_
  #define _BOOST_CSTDFLOAT_LIMITS_2014_01_09_HPP_

  #include <boost/math/cstdfloat/cstdfloat_types.hpp>

  #if defined(BOOST_CSTDFLOAT_HAS_INTERNAL_FLOAT128_T) && defined(BOOST_MATH_USE_FLOAT128) && !defined(BOOST_CSTDFLOAT_NO_LIBQUADMATH_SUPPORT)

    #include <limits>

    // Define the name of the global quadruple-precision function to be used for
    // calculating quiet_NaN() in the specialization of std::numeric_limits<>.
    #if defined(BOOST_INTEL)
      #define BOOST_CSTDFLOAT_FLOAT128_SQRT   __sqrtq
    #elif defined(__GNUC__)
      #define BOOST_CSTDFLOAT_FLOAT128_SQRT   sqrtq
    #endif

    // Forward declaration of the quadruple-precision square root function.
    extern "C" boost::math::cstdfloat::detail::float_internal128_t BOOST_CSTDFLOAT_FLOAT128_SQRT(boost::math::cstdfloat::detail::float_internal128_t) throw();

    namespace std
    {
      template<>
      class numeric_limits<boost::math::cstdfloat::detail::float_internal128_t>
      {
      public:
        BOOST_STATIC_CONSTEXPR bool                                                 is_specialized           = true;
        static                 boost::math::cstdfloat::detail::float_internal128_t  (min) () BOOST_NOEXCEPT  { return BOOST_CSTDFLOAT_FLOAT128_MIN; }
        static                 boost::math::cstdfloat::detail::float_internal128_t  (max) () BOOST_NOEXCEPT  { return BOOST_CSTDFLOAT_FLOAT128_MAX; }
        static                 boost::math::cstdfloat::detail::float_internal128_t  lowest() BOOST_NOEXCEPT  { return -(max)(); }
        BOOST_STATIC_CONSTEXPR int                                                  digits                   = 113;
        BOOST_STATIC_CONSTEXPR int                                                  digits10                 = 33;
        BOOST_STATIC_CONSTEXPR int                                                  max_digits10             = 36;
        BOOST_STATIC_CONSTEXPR bool                                                 is_signed                = true;
        BOOST_STATIC_CONSTEXPR bool                                                 is_integer               = false;
        BOOST_STATIC_CONSTEXPR bool                                                 is_exact                 = false;
        BOOST_STATIC_CONSTEXPR int                                                  radix                    = 2;
        static                 boost::math::cstdfloat::detail::float_internal128_t  epsilon    ()            { return BOOST_CSTDFLOAT_FLOAT128_EPS; }
        static                 boost::math::cstdfloat::detail::float_internal128_t  round_error()            { return BOOST_FLOAT128_C(0.5); }
        BOOST_STATIC_CONSTEXPR int                                                  min_exponent             = -16381;
        BOOST_STATIC_CONSTEXPR int                                                  min_exponent10           = static_cast<int>((min_exponent * 301L) / 1000L);
        BOOST_STATIC_CONSTEXPR int                                                  max_exponent             = +16384;
        BOOST_STATIC_CONSTEXPR int                                                  max_exponent10           = static_cast<int>((max_exponent * 301L) / 1000L);
        BOOST_STATIC_CONSTEXPR bool                                                 has_infinity             = true;
        BOOST_STATIC_CONSTEXPR bool                                                 has_quiet_NaN            = true;
        BOOST_STATIC_CONSTEXPR bool                                                 has_signaling_NaN        = false;
        BOOST_STATIC_CONSTEXPR float_denorm_style                                   has_denorm               = denorm_absent;
        BOOST_STATIC_CONSTEXPR bool                                                 has_denorm_loss          = false;
        static                 boost::math::cstdfloat::detail::float_internal128_t  infinity     ()          { return BOOST_FLOAT128_C(1.0) / BOOST_FLOAT128_C(0.0); }
        static                 boost::math::cstdfloat::detail::float_internal128_t  quiet_NaN    ()          { return ::BOOST_CSTDFLOAT_FLOAT128_SQRT(BOOST_FLOAT128_C(-1.0)); }
        static                 boost::math::cstdfloat::detail::float_internal128_t  signaling_NaN()          { return BOOST_FLOAT128_C(0.0); }
        static                 boost::math::cstdfloat::detail::float_internal128_t  denorm_min   ()          { return BOOST_FLOAT128_C(0.0); }
        BOOST_STATIC_CONSTEXPR bool                                                 is_iec559                = true;
        BOOST_STATIC_CONSTEXPR bool                                                 is_bounded               = false;
        BOOST_STATIC_CONSTEXPR bool                                                 is_modulo                = false;
        BOOST_STATIC_CONSTEXPR bool                                                 traps                    = false;
        BOOST_STATIC_CONSTEXPR bool                                                 tinyness_before          = false;
        BOOST_STATIC_CONSTEXPR float_round_style                                    round_style              = round_to_nearest;
      };
    } // namespace std

  #endif // Not BOOST_CSTDFLOAT_NO_LIBQUADMATH_SUPPORT (i.e., the user would like to have libquadmath support)

#endif // _BOOST_CSTDFLOAT_LIMITS_2014_01_09_HPP_


#include "space/shipentity.h"

#include <boost/log/trivial.hpp>
#include <glm/gtx/transform.hpp>
#include <glm/gtx/string_cast.hpp>
#include <glm/gtx/euler_angles.hpp>

space::ShipEntity::ShipEntity(float power):
  _power(power),
  _yawDelta(0),
  _rollDelta(0),
  _pitchDelta(0),
  _throttle(0),
  _rotate(1),
  _position(0, 0, 0)
{
}

void space::ShipEntity::setYawDelta(float val)
{
  _yawDelta = val;
}

void space::ShipEntity::setRollDelta(float val)
{
  _rollDelta = val;
}

void space::ShipEntity::setPitchDelta(float val)
{
  _pitchDelta = val;
}

void space::ShipEntity::setThrottle(float val)
{
  _throttle = val;
}

glm::mat4 space::ShipEntity::update(float deltaTs)
{
  // Adjust attitude
  _rotate *= glm::eulerAngleYXZ(_yawDelta * deltaTs,
                                _pitchDelta * deltaTs,
                                _rollDelta * deltaTs);

  // Adjust position
  glm::vec3 front(0, 0, 1);
  glm::vec3 up(0, 1, 0);
  _position +=
    (glm::mat3(_rotate) * front) *
    _power * deltaTs * _throttle;

  return glm::lookAt(_position,
                     _position + glm::mat3(_rotate) * front,
                     glm::mat3(_rotate) * up);
}


#include <iostream>
#include <mist/mist.h>
#include <Eigen/Core>
#include "ItkImageIO.h"
#include "dataIO.h"
#include "configs.h"
#include "em_algorithm.h"

typedef unsigned char mask_t;
typedef double feat_t;
typedef double atlas_t;

int main(int argc, char **argv) {

	if (argc != 7 && argc != 8) {
		std::cerr << "Usage:" << std::endl;
		std::cerr << argv[0] << " <input feature directory> <input abdominal cavity mask directory> "
			<< " <input param directory> <output directory> <test filename list> <feature name list> "
			<< " <input atlas directory>"
			<< std::endl;
		return EXIT_FAILURE;
	}

	std::cout << "----- Read data list -----" << std::endl;
	const std::string input_feat_dir = std::string(argv[1]) + "/";
	const std::string input_abd_mask_dir = std::string(argv[2]) + "/";
	const std::string input_param_dir = std::string(argv[3]) + "/";
	const std::string output_dir = std::string(argv[4]) + "/";
	const std::string test_filename_list_path = argv[5];
	const std::string feature_name_list_path = argv[6];
	std::string input_atlas_dir;
	if (argc == 8) {
		input_atlas_dir = std::string(argv[7]) + "/";
	}

	std::list<std::string> test_filename_list;
	std::list<std::string> feature_name_list;
	if (!get_data_list(test_filename_list_path, test_filename_list) ||
		!get_data_list(feature_name_list_path, feature_name_list)) {
		return EXIT_FAILURE;
	}
	const int num_cases = test_filename_list.size();
	const int num_features = feature_name_list.size();

	std::cout << "----- Load init param -----" << std::endl;
	std::vector<Eigen::MatrixXd> mean_init(TESTTIME_NUM_LABELS, Eigen::MatrixXd(num_features, 1));
	std::vector<Eigen::MatrixXd> covariance_init(TESTTIME_NUM_LABELS, Eigen::MatrixXd(num_features, num_features));
	for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
		read_raw_to_eigen(mean_init[l], input_param_dir + TESTTIME_LABEL_NAMES[l] + "/mean_param.raw", num_features);
		read_raw_to_eigen(covariance_init[l], input_param_dir + TESTTIME_LABEL_NAMES[l] + "/covariance_param.raw", num_features);
	}
	//for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
	//	std::cout << mean_init[l] << std::endl;
	//}

	std::cout << "----- Apply em algorithm -----" << std::endl;
	for (auto case_itr = test_filename_list.begin(); case_itr != test_filename_list.end(); ++case_itr) {
		std::cout << "Case: " << *case_itr << std::endl;
		std::vector<mask_t> abd_mask_img;
		ImageIO<NDIMS> abd_mask_mhd;
		abd_mask_mhd.Read(abd_mask_img, input_abd_mask_dir + *case_itr);
		const int xe = abd_mask_mhd.Size(0);
		const int ye = abd_mask_mhd.Size(1);
		const int ze = abd_mask_mhd.Size(2);
		const int se = xe*ye*ze;
		const double x_spacing = abd_mask_mhd.Spacing(0);
		const double y_spacing = abd_mask_mhd.Spacing(1);
		const double z_spacing = abd_mask_mhd.Spacing(2);

		size_t num_pixel = 0;
		for (int z = 0; z < ze; z++) {
			for (int y = 0; y < ye; y++) {
				for (int x = 0; x < xe; x++) {
					if (abd_mask_img.at(z * xe * ye + y * xe + x)) num_pixel++;
				}
			}
		}

		std::cout << "---- Load feature ----" << std::endl;
		std::vector<feat_t> feature_array(num_pixel * num_features);
		for(auto feat_itr = feature_name_list.begin(); feat_itr != feature_name_list.end(); ++feat_itr) {
			std::vector<feat_t> feat_img;
			ImageIO<NDIMS> feature_mhd;
			feature_mhd.Read(feat_img, input_feat_dir + *feat_itr + "/" + *case_itr);

			size_t tmp = 0;
			size_t i = std::distance(feature_name_list.begin(), feat_itr);
			for (int z = 0; z < ze; z++) {
				for (int y = 0; y < ye; y++) {
					for (int x = 0; x < xe; x++) {
						if (abd_mask_img.at(z * xe * ye + y * xe + x)) {
							feature_array.at(i*num_pixel + tmp) = feat_img.at(z * xe * ye + y * xe + x);
							tmp++;
						}
					}
				}
			}
		}

		std::vector<feat_t> atlas_array(num_pixel * TESTTIME_NUM_LABELS);
		if (argc == 8) { // Atlas distribution
			std::cout << "---- Load atlas ----" << std::endl;
			for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
				std::vector<atlas_t> atlas_img;
				ImageIO<NDIMS> atlas_mhd;
				atlas_mhd.Read(atlas_img, input_atlas_dir + TESTTIME_LABEL_NAMES[l] + "/atlas.mhd");

				size_t tmp = 0;
				for (int z = 0; z < ze; z++) {
					for (int y = 0; y < ye; y++) {
						for (int x = 0; x < xe; x++) {
							if (abd_mask_img.at(z * xe * ye + y * xe + x)) {
								atlas_array.at(l*num_pixel + tmp) = atlas_img.at(z * xe * ye + y * xe + x);
								tmp++;
							}
						}
					}
				}
			}/* Label loop */
			// If atlas is 0, label is assigned others class
			for (int n = 0; n < num_pixel; n++) {
				double tmp = 0.;
				for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
					tmp += atlas_array.at(l*num_pixel + n);
				}
				if (tmp <= 0) {
					atlas_array.at((TESTTIME_NUM_LABELS - 1)*num_pixel + n) = 1.;
				}
			}

		}
		else {
			// Uniform distribution
			for (int s = 0; s < num_pixel*TESTTIME_NUM_LABELS; s++) {
				atlas_array.at(s) = 1.0 / (atlas_t)TESTTIME_NUM_LABELS;
			}
		}

		std::cout << "----- Start EM algorithm -----" << std::endl;
		std::vector<Eigen::MatrixXd> mean(mean_init);
		std::vector<Eigen::MatrixXd> covariance(covariance_init);
		size_t iteration_counter;
		em_algorithm::estimate_mixture_using_atlas(feature_array,
													mean,
													covariance,
													atlas_array,
													num_pixel,
													num_features,
													TESTTIME_NUM_LABELS,
													100,
													0.001,
													iteration_counter);

		std::cout << "----- Save estimated param -----" << std::endl;
		for (int l = 0; l < TESTTIME_NUM_LABELS; l++) {
			std::string result_dir = output_dir + TESTTIME_LABEL_NAMES[l];
			make_dir(result_dir);
			save_param_as_csv(mean[l], result_dir + "/estimated_mean_param.csv");
			save_param_as_csv(covariance[l], result_dir + "/estimated_covariance_param.csv");
			write_raw_and_txt(mean[l], result_dir + "/estimated_mean_param");
			write_raw_and_txt(covariance[l], result_dir + "/estimated_covariance_param");
		} /* Label loop */

	}/* Case loop */

	return EXIT_SUCCESS;
}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Irstea
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *   * Neither the name of Irstea 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 <four_wheel_steering_controller/odometry.h>

#include <boost/bind.hpp>

namespace four_wheel_steering_controller
{
  namespace bacc = boost::accumulators;

  Odometry::Odometry(size_t velocity_rolling_window_size)
  : last_update_timestamp_(0.0)
  , x_(0.0)
  , y_(0.0)
  , heading_(0.0)
  , linear_(0.0)
  , linear_x_(0.0)
  , linear_y_(0.0)
  , angular_(0.0)
  , steering_track_(0.0)
  , wheel_steering_y_offset_(0.0)
  , wheel_radius_(0.0)
  , wheel_base_(0.0)
  , wheel_old_pos_(0.0)
  , velocity_rolling_window_size_(velocity_rolling_window_size)
  , linear_accel_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , linear_jerk_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , front_steer_vel_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  , rear_steer_vel_acc_(RollingWindow::window_size = velocity_rolling_window_size)
  {
  }

  void Odometry::init(const ros::Time& time)
  {
    // Reset accumulators and timestamp:
    resetAccumulators();
    last_update_timestamp_ = time;
  }

  bool Odometry::update(const double &fl_speed, const double &fr_speed,
                        const double &rl_speed, const double &rr_speed,
                        double front_steering, double rear_steering, const ros::Time &time)
  {
    const double front_tmp = cos(front_steering)*(tan(front_steering)-tan(rear_steering))/wheel_base_;
    const double front_left_tmp = front_tmp/sqrt(1-steering_track_*front_tmp*cos(front_steering)
                                               +pow(steering_track_*front_tmp/2,2));
    const double front_right_tmp = front_tmp/sqrt(1+steering_track_*front_tmp*cos(front_steering)
                                                +pow(steering_track_*front_tmp/2,2));
    const double fl_speed_tmp = fl_speed * (1/(1-wheel_steering_y_offset_*front_left_tmp));
    const double fr_speed_tmp = fr_speed * (1/(1-wheel_steering_y_offset_*front_right_tmp));
    const double front_linear_speed = wheel_radius_ * copysign(1.0, fl_speed_tmp+fr_speed_tmp)*
        sqrt((pow(fl_speed,2)+pow(fr_speed,2))/(2+pow(steering_track_*front_tmp,2)/2.0));

    const double rear_tmp = cos(rear_steering)*(tan(front_steering)-tan(rear_steering))/wheel_base_;
    const double rear_left_tmp = rear_tmp/sqrt(1-steering_track_*rear_tmp*cos(rear_steering)
                                               +pow(steering_track_*rear_tmp/2,2));
    const double rear_right_tmp = rear_tmp/sqrt(1+steering_track_*rear_tmp*cos(rear_steering)
                                                +pow(steering_track_*rear_tmp/2,2));
    const double rl_speed_tmp = rl_speed * (1/(1-wheel_steering_y_offset_*rear_left_tmp));
    const double rr_speed_tmp = rr_speed * (1/(1-wheel_steering_y_offset_*rear_right_tmp));
    const double rear_linear_speed = wheel_radius_ * copysign(1.0, rl_speed_tmp+rr_speed_tmp)*
        sqrt((pow(rl_speed_tmp,2)+pow(rr_speed_tmp,2))/(2+pow(steering_track_*rear_tmp,2)/2.0));

    angular_ = (front_linear_speed*front_tmp + rear_linear_speed*rear_tmp)/2.0;

    linear_x_ = (front_linear_speed*cos(front_steering) + rear_linear_speed*cos(rear_steering))/2.0;
    linear_y_ = (front_linear_speed*sin(front_steering) - wheel_base_*angular_/2.0
                + rear_linear_speed*sin(rear_steering) + wheel_base_*angular_/2.0)/2.0;
    linear_ =  copysign(1.0, rear_linear_speed)*sqrt(pow(linear_x_,2)+pow(linear_y_,2));

    /// Compute x, y and heading using velocity
    const double dt = (time - last_update_timestamp_).toSec();
    if (dt < 0.0001)
      return false; // Interval too small to integrate with

    last_update_timestamp_ = time;
    /// Integrate odometry:
    integrateXY(linear_x_*dt, linear_y_*dt, angular_*dt);

    linear_accel_acc_((linear_vel_prev_ - linear_)/dt);
    linear_vel_prev_ = linear_;
    linear_jerk_acc_((linear_accel_prev_ - bacc::rolling_mean(linear_accel_acc_))/dt);
    linear_accel_prev_ = bacc::rolling_mean(linear_accel_acc_);
    front_steer_vel_acc_((front_steer_vel_prev_ - front_steering)/dt);
    front_steer_vel_prev_ = front_steering;
    rear_steer_vel_acc_((rear_steer_vel_prev_ - rear_steering)/dt);
    rear_steer_vel_prev_ = rear_steering;
    return true;
  }

  void Odometry::setWheelParams(double steering_track, double wheel_steering_y_offset, double wheel_radius, double wheel_base)
  {
    steering_track_   = steering_track;
    wheel_steering_y_offset_ = wheel_steering_y_offset;
    wheel_radius_     = wheel_radius;
    wheel_base_       = wheel_base;
  }

  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
  {
    velocity_rolling_window_size_ = velocity_rolling_window_size;

    resetAccumulators();
  }

  void Odometry::integrateXY(double linear_x, double linear_y, double angular)
  {
    const double delta_x = linear_x*cos(heading_) - linear_y*sin(heading_);
    const double delta_y = linear_x*sin(heading_) + linear_y*cos(heading_);

    x_ += delta_x;
    y_ += delta_y;
    heading_ += angular;
  }

  void Odometry::integrateRungeKutta2(double linear, double angular)
  {
    const double direction = heading_ + angular * 0.5;

    /// Runge-Kutta 2nd order integration:
    x_       += linear * cos(direction);
    y_       += linear * sin(direction);
    heading_ += angular;
  }

  void Odometry::integrateExact(double linear, double angular)
  {
    if (fabs(angular) < 1e-6)
      integrateRungeKutta2(linear, angular);
    else
    {
      /// Exact integration (should solve problems when angular is zero):
      const double heading_old = heading_;
      const double r = linear/angular;
      heading_ += angular;
      x_       +=  r * (sin(heading_) - sin(heading_old));
      y_       += -r * (cos(heading_) - cos(heading_old));
    }
  }

  void Odometry::resetAccumulators()
  {
    linear_accel_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
    linear_jerk_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
    front_steer_vel_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
    rear_steer_vel_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
  }

} // namespace four_wheel_steering_controller


#include <boost/test/unit_test.hpp>
#include "functions/addition.hh"
#include "functions/variables.hh"
#include "functions/full_function_defs.hh"
#include "functions/std_functions.hh"
#include "functions/polynomial.hh"
#include "functions/operators.hh"
#include <iostream>
#include "functions/streaming.hh"

BOOST_AUTO_TEST_CASE(addition_test) {
  using namespace manifolds;
  Variable<1> b1;
  Variable<2> b2;
  Variable<4> b3;
  Variable<6> m1;
  Variable<7> m2;
  Variable<8> m3;
  auto c33 = b1 + m1 * t;
  auto c22 = b2 + m2 * t;
  auto c11 = b3 + m3 * t;
  auto part1 = 4_c * c22 * c22 * c11 * c33;
  /*CustomVariableNamer
  vnamer{ std::make_pair(1, "b1"), std::make_pair(2, "b2"),
          std::make_pair(4, "b3"), std::make_pair(6, "m1"),
          std::make_pair(7, "m2"), std::make_pair(8, "m3"), };*/
  // Stream2(std::cout, part1, vnamer) << "\n\n";
  (void)part1;
}


#pragma once

#include "Concepts.hpp"
#include "State.hpp"

#include <boost/hana/concat.hpp>

#include <span>

namespace codys {

template <class Lhs, class Rhs, class Unit_>
struct Add {
    using depends_on =
        decltype(boost::hana::concat(std::declval<typename Lhs::depends_on>(),
                                     std::declval<typename Rhs::depends_on>()));
    using Unit = Unit_;

    Lhs lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.template evaluate<SystemType>(arr) +
               rhs.template evaluate<SystemType>(arr);
    }
};

template <class Lhs, class Rhs, class Unit>
constexpr auto operator+([[maybe_unused]] State<Lhs, Unit> lhs,
                         [[maybe_unused]] State<Rhs, Unit> rhs) {
    return Add<State<Lhs, Unit>, State<Rhs, Unit>, Unit>{lhs, rhs};
}

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
struct AddScalar {
    using depends_on = typename Rhs::depends_on;
    using Unit = decltype(std::declval<Scalar>() + std::declval<UnitRhs>());

    Scalar lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.number() + rhs.template evaluate<SystemType>(arr);
    }
};

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
constexpr auto operator+([[maybe_unused]] Scalar lhs,
                         [[maybe_unused]] State<Rhs, UnitRhs> rhs) {
    return AddScalar<Scalar, State<Rhs, UnitRhs>, UnitRhs>{lhs, rhs};
}

template <PhysicalValue Scalar, class Lhs, class UnitLhs>
constexpr auto operator+([[maybe_unused]] State<Lhs, UnitLhs> lhs,
                         [[maybe_unused]] Scalar rhs) {
    return AddScalar<Scalar, State<Lhs, UnitLhs>, UnitLhs>{rhs, lhs};
}

template <class Lhs, class Rhs, class Unit_>
struct Substract {
    using depends_on =
        decltype(boost::hana::concat(std::declval<typename Lhs::depends_on>(),
                                     std::declval<typename Rhs::depends_on>()));
    using Unit = Unit_;

    Lhs lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.template evaluate<SystemType>(arr) -
               rhs.template evaluate<SystemType>(arr);
    }
};

template <class Lhs, class Rhs, class Unit>
constexpr auto operator-([[maybe_unused]] State<Lhs, Unit> lhs,
                         [[maybe_unused]] State<Rhs, Unit> rhs) {
    return Substract<State<Lhs, Unit>, State<Rhs, Unit>, Unit>{lhs, rhs};
}

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
struct SubstractScalarFromLeft {
    using depends_on = typename Rhs::depends_on;
    using Unit = decltype(std::declval<Scalar>() - std::declval<UnitRhs>());

    Scalar lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.number() - rhs.template evaluate<SystemType>(arr);
    }
};

template <class Lhs, class UnitLhs, PhysicalValue Scalar>
struct SubstractScalarFromRight {
    using depends_on = typename Lhs::depends_on;
    using Unit = decltype(std::declval<UnitLhs>() - std::declval<Scalar>());

    Lhs lhs;
    Scalar rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.template evaluate<SystemType>(arr) - rhs.number();
    }
};

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
constexpr auto operator-([[maybe_unused]] Scalar lhs,
                         [[maybe_unused]] State<Rhs, UnitRhs> rhs) {
    return SubstractScalarFromLeft<Scalar, State<Rhs, UnitRhs>, UnitRhs>{lhs, rhs};
}

template <class Lhs, class UnitLhs, PhysicalValue Scalar>
constexpr auto operator-([[maybe_unused]] State<Lhs, UnitLhs> lhs,
                         [[maybe_unused]] Scalar rhs) {
    return SubstractScalarFromRight<State<Lhs, UnitLhs>, UnitLhs, Scalar>{lhs, rhs};
}

template <class Lhs, class Rhs, class UnitLhs, class UnitRhs>
struct Multiply {
    using depends_on =
        decltype(boost::hana::concat(std::declval<typename Lhs::depends_on>(),
                                     std::declval<typename Rhs::depends_on>()));
    using Unit = decltype(std::declval<UnitLhs>() * std::declval<UnitRhs>());

    Lhs lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.template evaluate<SystemType>(arr) *
               rhs.template evaluate<SystemType>(arr);
    }
};

template <class Lhs, class Rhs, class UnitLhs, class UnitRhs>
constexpr auto operator*([[maybe_unused]] State<Lhs, UnitLhs> lhs,
                         [[maybe_unused]] State<Rhs, UnitRhs> rhs) {
    return Multiply<State<Lhs, UnitLhs>, State<Rhs, UnitRhs>, UnitLhs, UnitRhs>{lhs, rhs};
}

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
struct MultiplyScalar {
    using depends_on = typename Rhs::depends_on;
    using Unit = decltype(std::declval<Scalar>() * std::declval<UnitRhs>());

    Scalar lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.number() * rhs.template evaluate<SystemType>(arr);
    }
};

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
constexpr auto operator*([[maybe_unused]] Scalar lhs,
                         [[maybe_unused]] State<Rhs, UnitRhs> rhs) {
    return MultiplyScalar<Scalar, State<Rhs, UnitRhs>, UnitRhs>{lhs, rhs};
}

template <PhysicalValue Scalar, class Lhs, class UnitLhs>
constexpr auto operator*([[maybe_unused]] State<Lhs, UnitLhs> lhs,
                         [[maybe_unused]] Scalar rhs) {
    return MultiplyScalar<Scalar, State<Lhs, UnitLhs>, UnitLhs>{rhs, lhs};
}

template <class Lhs, class Rhs, class UnitLhs, class UnitRhs>
struct Divide {
    using depends_on =
        decltype(boost::hana::concat(std::declval<typename Lhs::depends_on>(),
                                     std::declval<typename Rhs::depends_on>()));
    using Unit = decltype(std::declval<UnitLhs>() / std::declval<UnitRhs>());

    Lhs lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.template evaluate<SystemType>(arr) /
               rhs.template evaluate<SystemType>(arr);
    }
};

template <class Lhs, class Rhs, class UnitLhs, class UnitRhs>
constexpr auto operator/([[maybe_unused]] State<Lhs, UnitLhs> lhs,
                         [[maybe_unused]] State<Rhs, UnitRhs> rhs) {
    return Divide<State<Lhs, UnitLhs>, State<Rhs, UnitRhs>, UnitLhs, UnitRhs>{lhs, rhs};
}

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
struct DivideScalarFromLeft {
    using depends_on = typename Rhs::depends_on;
    using Unit = decltype(std::declval<Scalar>() / std::declval<UnitRhs>());

    Scalar lhs;
    Rhs rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.number() / rhs.template evaluate<SystemType>(arr);
    }
};

template <class Lhs, class UnitLhs, PhysicalValue Scalar>
struct DivideScalarFromRight {
    using depends_on = typename Lhs::depends_on;
    using Unit = decltype(std::declval<UnitLhs>() / std::declval<Scalar>());

    Lhs lhs;
    Scalar rhs;

    template <class SystemType, std::size_t N>
    constexpr double evaluate(std::span<const double, N> arr) const {
        return lhs.template evaluate<SystemType>(arr) / rhs.number();
    }
};

template <PhysicalValue Scalar, class Rhs, class UnitRhs>
constexpr auto operator/([[maybe_unused]] Scalar lhs,
                         [[maybe_unused]] State<Rhs, UnitRhs> rhs) {
    return DivideScalarFromLeft<Scalar, State<Rhs, UnitRhs>, UnitRhs>{lhs, rhs};
}

template <class Lhs, class UnitLhs, PhysicalValue Scalar>
constexpr auto operator/([[maybe_unused]] State<Lhs, UnitLhs> lhs,
                         [[maybe_unused]] Scalar rhs) {
    return DivideScalarFromRight<State<Lhs, UnitLhs>, UnitLhs, Scalar>{lhs, rhs};
}

} // namespace codys


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_TASKABSTRACT_H__
#define __SOT_TASKABSTRACT_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>

/* STD */
#include <string>

/* SOT */
#include "sot/core/api.hh"
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/multi-bound.hh>

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

namespace dynamicgraph {
namespace sot {

/// Hierarchical element of the stack of tasks.
///
/// A task computes a value and a Jacobian as output signals.
/// Once stacked into a solver, the solver will compute the control
/// vector that makes the task values converge toward zero in the
/// order defined by the priority levels.
///
/// \image html pictures/task.png "Task diagram: Task types derive from
/// TaskAbstract. The value and Jacobian of a Task are computed from the
/// features that are stored in the task.

class SOT_CORE_EXPORT TaskAbstract : public dynamicgraph::Entity {
public:
  /* Use a derivative of this class to store computational memory. */
  class MemoryTaskAbstract {
  public:
    int timeLastChange;

  public:
    MemoryTaskAbstract(void) : timeLastChange(0){};
    virtual ~MemoryTaskAbstract(void){};

  public:
    virtual void display(std::ostream &os) const = 0;
    friend std::ostream &operator<<(std::ostream &os,
                                    const MemoryTaskAbstract &tcm) {
      tcm.display(os);
      return os;
    }
  };

public:
  MemoryTaskAbstract *memoryInternal;

protected:
  void taskRegistration(void);

public:
  TaskAbstract(const std::string &n);

public: /* --- SIGNALS --- */
  dynamicgraph::SignalTimeDependent<VectorMultiBound, int> taskSOUT;
  dynamicgraph::SignalTimeDependent<dynamicgraph::Matrix, int> jacobianSOUT;
};

} /* namespace sot */
} /* namespace dynamicgraph */

#endif /* #ifndef __SOT_TASKABSTRACT_H__ */


/**
 * Copyright 2020-2022 Huawei Technologies Co., Ltd
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "plugin/device/cpu/kernel/eigen/matrix_triangular_solve_cpu_kernel.h"
#include <Eigen/Dense>
#include <vector>
#include <string>
namespace mindspore {
namespace kernel {
using Eigen::ColMajor;
using Eigen::Dynamic;
using Eigen::Lower;
using Eigen::Map;
using Eigen::MatrixBase;
using Eigen::RowMajor;
using Eigen::UnitLower;
using Eigen::UnitUpper;
using Eigen::Upper;
template <typename T, int Major>
using Matrix = Eigen::Matrix<T, Dynamic, Dynamic, Major>;
constexpr auto kSolveTriangularInputsNum = 2;
constexpr auto kSolveTriangularOutputsNum = 1;
constexpr auto kAVectorxDimNum = 1;
constexpr auto kAMatrixDimNum = 2;
constexpr size_t kRowIndex = 2;
constexpr size_t kColIndex = 1;
template <typename T>
void MatrixTriangularSolveCpuKernelMod<T>::InitShape(const CNodePtr &kernel_node) {
  auto a_shape = common::AnfAlgo::GetPrevNodeOutputInferShape(kernel_node, 0);
  auto b_shape = common::AnfAlgo::GetPrevNodeOutputInferShape(kernel_node, 1);
  // Since the shape check is done in frontend, we can suppose that the shape of a, b here is valid.
  size_t a_dims = a_shape.size();
  size_t aRowIndex = a_dims - kRowIndex;
  m_ = a_shape[aRowIndex];
  size_t b_sims = b_shape.size();
  bool vector_b = b_sims == a_dims - 1;
  if (vector_b) {
    n_ = 1;
  } else {
    n_ = b_shape[b_sims - 1];
  }
  batch_ = 1;
  for (size_t batch = 0; batch < a_dims - kRowIndex; ++batch) {
    batch_ *= a_shape[batch];
  }
}

template <typename T>
void MatrixTriangularSolveCpuKernelMod<T>::InitKernel(const CNodePtr &kernel_node) {
  kernel_name_ = common::AnfAlgo::GetCNodeName(kernel_node);
  InitShape(kernel_node);
  if (common::AnfAlgo::HasNodeAttr(ADJOINT, kernel_node)) {
    // MatrixTriangularSolve attribute
    trans_ = common::AnfAlgo::GetNodeAttr<bool>(kernel_node, ADJOINT);
    if (common::AnfAlgo::HasNodeAttr(TRANS, kernel_node)) {
      MS_LOG(EXCEPTION) << "For '" << kernel_name_
                        << "', the attribute 'adjoint' and 'trans' could not exist at the same time.";
    }
  } else {
    lower_ = common::AnfAlgo::GetNodeAttr<bool>(kernel_node, LOWER);
    unit_diagonal_ = common::AnfAlgo::GetNodeAttr<bool>(kernel_node, UNIT_DIAGONAL);
    const std::string trans = common::AnfAlgo::GetNodeAttr<std::string>(kernel_node, TRANS);
    if (trans == "N") {
      trans_ = false;
    } else if (trans == "T") {
      trans_ = true;
    } else if (trans == "C") {
      trans_ = true;
    } else {
      MS_LOG(EXCEPTION) << "For '" << kernel_name_ << "', 'trans' should be in ['N', 'T', 'C'], but got [" << trans
                        << "].";
    }
  }
}

template <typename Derived_a, typename Derived_b, typename T>
inline void solve(const MatrixBase<Derived_a> &a, const MatrixBase<Derived_b> &b, T *output_addr, int m, int n,
                  bool lower, bool unit_diagonal) {
  Map<Matrix<T, RowMajor>> output(output_addr, m, n);
  if (unit_diagonal) {
    if (lower) {
      output.noalias() = a.template triangularView<UnitLower>().solve(b);
    } else {
      output.noalias() = a.template triangularView<UnitUpper>().solve(b);
    }
  } else {
    if (lower) {
      output.noalias() = a.template triangularView<Lower>().solve(b);
    } else {
      output.noalias() = a.template triangularView<Upper>().solve(b);
    }
  }
}

template <typename T>
bool MatrixTriangularSolveCpuKernelMod<T>::Launch(const std::vector<AddressPtr> &inputs,
                                                  const std::vector<AddressPtr> & /* workspace */,
                                                  const std::vector<AddressPtr> &outputs) {
  CHECK_KERNEL_INPUTS_NUM(inputs.size(), kSolveTriangularInputsNum, kernel_name_);
  CHECK_KERNEL_OUTPUTS_NUM(outputs.size(), kSolveTriangularOutputsNum, kernel_name_);

  auto a_addr = reinterpret_cast<T *>(inputs[0]->addr);
  auto b_addr = reinterpret_cast<T *>(inputs[1]->addr);
  auto output_addr = reinterpret_cast<T *>(outputs[0]->addr);

  size_t a_batch_size = m_ * m_;
  size_t b_batch_size = m_ * n_;
  size_t output_batch_size = m_ * n_;

  for (size_t i = 0; i < batch_; ++i) {
    T *a_batch_addr = a_addr + i * a_batch_size;
    T *b_batch_addr = b_addr + i * b_batch_size;
    T *output_batch_addr = output_addr + i * output_batch_size;

    Map<Matrix<T, RowMajor>> b(b_batch_addr, m_, n_);
    if (trans_) {
      Map<Matrix<T, ColMajor>> a(a_batch_addr, m_, m_);
      solve(a, b, output_batch_addr, m_, n_, !lower_, unit_diagonal_);
    } else {
      Map<Matrix<T, RowMajor>> a(a_batch_addr, m_, m_);
      solve(a, b, output_batch_addr, m_, n_, lower_, unit_diagonal_);
    }
  }

  return true;
}
}  // namespace kernel
}  // namespace mindspore


//==============================================================================
// Copyright 2018-2020 Kitware, Inc., Kitware SAS
// Author: Guilbert Pierre (Kitware SAS)
//         Laurenson Nick (Kitware SAS)
//         Cadart Nicolas (Kitware SAS)
// Creation date: 2018-03-27
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//==============================================================================

#include "LidarSlam/Utilities.h"
#include "LidarSlam/SpinningSensorKeypointExtractor.h"

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace LidarSlam
{

namespace
{
//-----------------------------------------------------------------------------
struct LineFitting
{
  //! Fitting using PCA
  bool FitPCA(const SpinningSensorKeypointExtractor::PointCloud& cloud,
              const std::vector<int>& indices);

  //! Fitting using very local line and check if this local line is consistent
  //! in a more global neighborhood
  bool FitPCAAndCheckConsistency(const SpinningSensorKeypointExtractor::PointCloud& cloud,
                                 const std::vector<int>& indices);

  //! Compute the squared distance of a point to the fitted line
  inline float SquaredDistanceToPoint(Eigen::Vector3f const& point) const;

  // Direction and position
  Eigen::Vector3f Direction;
  Eigen::Vector3f Position;

  //! Max distance allowed from the farest point to estimated line to be considered as real line
  float MaxDistance = 0.02;  // [m]

  //! Max angle allowed between consecutive segments in the neighborhood to be considered as line
  float MaxAngle = DEG2RAD(40.);  // [rad]
};

//-----------------------------------------------------------------------------
bool LineFitting::FitPCA(const SpinningSensorKeypointExtractor::PointCloud& cloud,
                         const std::vector<int>& indices)
{
  // Compute PCA to determine best line approximation of the points distribution
  // and save points centroid in Position
  Eigen::Vector3f eigVals;
  Eigen::Matrix3f eigVecs;
  Utils::ComputeMeanAndPCA(cloud, indices, this->Position, eigVecs, eigVals);

  // Get Direction as main eigen vector
  this->Direction = eigVecs.col(2);

  // If a point of the neighborhood is too far from the fitted line,
  // we consider the neighborhood as non flat
  bool isLineFittingAccurate = true;
  const float sqMaxDistance = this->MaxDistance * this->MaxDistance;
  for (const auto& pointId: indices)
  {
    if (this->SquaredDistanceToPoint(cloud[pointId].getVector3fMap()) > sqMaxDistance)
    {
      isLineFittingAccurate = false;
      break;
    }
  }
  return isLineFittingAccurate;
}

//-----------------------------------------------------------------------------
bool LineFitting::FitPCAAndCheckConsistency(const SpinningSensorKeypointExtractor::PointCloud& cloud,
                                            const std::vector<int>& indices)
{
  const float maxSinAngle = std::sin(this->MaxAngle);
  bool isLineFittingAccurate = true;

  // First check if the neighborhood is approximately straight
  const Eigen::Vector3f U = (cloud[indices.back()].getVector3fMap() - cloud[indices.front()].getVector3fMap()).normalized();
  for (unsigned int i = 0; i < indices.size() - 1; i++)
  {
    const Eigen::Vector3f V = (cloud[indices[i + 1]].getVector3fMap() - cloud[indices[i]].getVector3fMap()).normalized();
    const float sinAngle = (U.cross(V)).norm();
    if (sinAngle > maxSinAngle)
    {
      isLineFittingAccurate = false;
      break;
    }
  }

  // Then fit with PCA (only if isLineFittingAccurate is true)
  return isLineFittingAccurate && this->FitPCA(cloud, indices);
}

//-----------------------------------------------------------------------------
inline float LineFitting::SquaredDistanceToPoint(Eigen::Vector3f const& point) const
{
  return ((point - this->Position).cross(this->Direction)).squaredNorm();
}
} // end of anonymous namespace

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ComputeKeyPoints(const PointCloud::Ptr& pc)
{
  this->Scan = pc;

  // Split whole pointcloud into separate laser ring clouds
  this->ConvertAndSortScanLines();

  // Initialize the features vectors and keypoints
  this->PrepareDataForNextFrame();

  // Invalidate points with bad criteria
  this->InvalidateNotUsablePoints();

  // Compute keypoints scores
  this->ComputeCurvature();

  // Labelize keypoints
  this->SetKeyPointsLabels();
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ConvertAndSortScanLines()
{
  // Clear previous scan lines
  for (auto& scanLineCloud: this->ScanLines)
  {
    // Use clear() if pointcloud already exists to avoid re-allocating memory.
    // No worry as ScanLines is never shared with outer scope.
    if (scanLineCloud)
      scanLineCloud->clear();
    else
      scanLineCloud.reset(new PointCloud);
  }

  // Separate pointcloud into different scan lines
  for (const Point& point: *this->Scan)
  {
    // Ensure that there are enough available scan lines
    while (point.laser_id >= this->ScanLines.size())
      this->ScanLines.emplace_back(new PointCloud);

    // Add the current point to its corresponding laser scan
    this->ScanLines[point.laser_id]->push_back(point);
  }

  // Save the number of lasers
  this->NbLaserRings = this->ScanLines.size();

  // Estimate azimuthal resolution if not already done
  // or if the previous value found is not plausible
  // (because last scan was badly formed, e.g. lack of points)
  if (this->AzimuthalResolution < 1e-6 || M_PI/4. < this->AzimuthalResolution)
    this->EstimateAzimuthalResolution();
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::PrepareDataForNextFrame()
{
  // Do not use clear(), otherwise weird things could happen
  // if outer program uses these pointers
  for (auto k : KeypointTypes)
  {
    this->Keypoints[k].reset(new PointCloud);
    Utils::CopyPointCloudMetadata(*this->Scan, *this->Keypoints[k]);
  }

  // Initialize the features vectors with the correct length
  this->Angles.resize(this->NbLaserRings);
  this->Saliency.resize(this->NbLaserRings);
  this->DepthGap.resize(this->NbLaserRings);
  this->IntensityGap.resize(this->NbLaserRings);
  this->IsPointValid.resize(this->NbLaserRings);
  this->Label.resize(this->NbLaserRings);

  // Initialize the scan lines features vectors with the correct length
  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided)
  for (int scanLine = 0; scanLine < static_cast<int>(this->NbLaserRings); ++scanLine)
  {
    size_t nbPoint = this->ScanLines[scanLine]->size();
    this->IsPointValid[scanLine].assign(nbPoint, KeypointFlags().set());  // set all flags to 1
    this->Label[scanLine].assign(nbPoint, KeypointFlags().reset());  // set all flags to 0
    this->Angles[scanLine].assign(nbPoint, 0.);
    this->Saliency[scanLine].assign(nbPoint, 0.);
    this->DepthGap[scanLine].assign(nbPoint, 0.);
    this->IntensityGap[scanLine].assign(nbPoint, 0.);
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::InvalidateNotUsablePoints()
{
  // Max angle between Lidar Ray and hyptothetic plane normal
  const float angleBeamNormal = Utils::Deg2Rad(90 - this->MinBeamSurfaceAngle);
  // If the azimuthal angle was estimated and is plausible,
  // it is used to invalidate points representing occluded areas border.
  // Otherwise, we use a default angle (Velodyne 10Hz resolution)
  float azimuthalResolution = this->AzimuthalResolution;
  if (azimuthalResolution < 1e-6 || M_PI / 4 < azimuthalResolution)
  {
    PRINT_WARNING("Unable to estimate the azimuthal resolution angle: using 0.2°");
    azimuthalResolution = Utils::Deg2Rad(0.2);
  }
  // Coeff to multiply to point depth, in order to obtain the maximal distance
  // between two neighbors of the same Lidar ray on a plane
  const float maxPosDiffCoeff = std::sin(azimuthalResolution) / std::cos(azimuthalResolution + angleBeamNormal);

  // Loop over scan lines
  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided) firstprivate(maxPosDiffCoeff)
  for (int scanLine = 0; scanLine < static_cast<int>(this->NbLaserRings); ++scanLine)
  {
    // Useful shortcuts
    const PointCloud& scanLineCloud = *(this->ScanLines[scanLine]);
    const int Npts = scanLineCloud.size();

    // If the line is almost empty, skip it
    if (this->IsScanLineAlmostEmpty(Npts))
    {
      for (int index = 0; index < Npts; ++index)
        this->IsPointValid[scanLine][index].reset();
      continue;
    }

    // Invalidate first and last points: because of undistortion, the depth of
    // first and last points can not be compared
    for (int index = 0; index < this->NeighborWidth; ++index)
    {
      this->IsPointValid[scanLine][index].reset();
      this->IsPointValid[scanLine][Npts - 1 - index].reset();
    }

    // Loop over remaining points of the scan line
    for (int index = this->NeighborWidth; index < Npts - this->NeighborWidth; ++index)
    {
      const auto& currentPoint = scanLineCloud[index].getVector3fMap();
      const float L = currentPoint.norm();
      // Invalidate points which are too close from the sensor
      if (L < this->MinDistanceToSensor)
      {
        this->IsPointValid[scanLine][index].reset();
      }

      // Compute maximal acceptable distance of two consecutive neighbors
      // aquired by the same laser, considering they lay on the same plane which
      // is not too oblique relatively to Lidar ray.
      // Check that this expected distance is not below range measurements noise.
      const float maxPosDiff = std::max(L * maxPosDiffCoeff, 0.02f);
      const float sqMaxPosDiff = maxPosDiff * maxPosDiff;

      // Invalidate occluded points due to depth gap or parallel beam.
      // If the distance between two successive points is bigger than the
      // expected length, it means that there is a depth gap. In this case, we
      // must invalidate the farthests points which belong to the occluded area.
      const auto& nextPoint = scanLineCloud[index + 1].getVector3fMap();
      if ((nextPoint - currentPoint).squaredNorm() > sqMaxPosDiff)
      {
        // If current point is the closest, next part is invalidated, starting from next point
        if (L < nextPoint.norm())
        {
          this->IsPointValid[scanLine][index + 1].reset();
          for (int i = index + 1; i < index + this->NeighborWidth; ++i)
          {
            const auto& Y  = scanLineCloud[i].getVector3fMap();
            const auto& Yn = scanLineCloud[i + 1].getVector3fMap();
            // If there is a new gap in the neighborhood,
            // the remaining points of the neighborhood are kept.
            if ((Yn - Y).squaredNorm() > sqMaxPosDiff)
              break;
            // Otherwise, the current neighbor point is disabled
            this->IsPointValid[scanLine][i + 1].reset();
          }
        }
        // If current point is the farthest, invalidate previous part, starting from current point
        else
        {
          this->IsPointValid[scanLine][index].reset();
          for (int i = index - 1; i > index - this->NeighborWidth; --i)
          {
            const auto& Yp = scanLineCloud[i].getVector3fMap();
            const auto&  Y = scanLineCloud[i + 1].getVector3fMap();
            // If there is a new gap in the neighborhood,
            // the remaining points of the neighborhood are kept.
            if ((Y - Yp).squaredNorm() > sqMaxPosDiff)
              break;
            // Otherwise, the previous neighbor point is disabled
            this->IsPointValid[scanLine][i].reset();
          }
        }
      }
    }
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ComputeCurvature()
{
  const float sqDistToLineThreshold = this->DistToLineThreshold * this->DistToLineThreshold;  // [m²]
  const float sqDepthDistCoeff = 0.25;
  const float minDepthGapDist = 1.5;  // [m]

  // loop over scans lines
  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided) \
          firstprivate(sqDistToLineThreshold, sqDepthDistCoeff, minDepthGapDist)
  for (int scanLine = 0; scanLine < static_cast<int>(this->NbLaserRings); ++scanLine)
  {
    // Useful shortcuts
    const PointCloud& scanLineCloud = *(this->ScanLines[scanLine]);
    const int Npts = scanLineCloud.size();

    // if the line is almost empty, skip it
    if (this->IsScanLineAlmostEmpty(Npts))
    {
      continue;
    }

    // loop over points in the current scan line
    // TODO : deal with spherical case : index=0 is neighbor of index=Npts-1
    for (int index = this->NeighborWidth; (index + this->NeighborWidth) < Npts; ++index)
    {
      // Skip curvature computation for invalid points
      if (this->IsPointValid[scanLine][index].none())
      {
        continue;
      }

      // central point
      const Point& currentPoint = scanLineCloud[index];
      const Eigen::Vector3f centralPoint = currentPoint.getVector3fMap();

      // compute intensity gap
      // CHECK : do not use currentPoint.intensity?
      const Point& previousPoint = scanLineCloud[index - 1];
      const Point& nextPoint = scanLineCloud[index + 1];
      this->IntensityGap[scanLine][index] = std::abs(nextPoint.intensity - previousPoint.intensity);

      // We will compute the line that fits the neighbors located before the current point.
      // We will do the same for the neighbors located after the current point.
      // We will then compute the angle between these two lines as an approximation
      // of the "sharpness" of the current point.
      std::vector<int> leftNeighbors(this->NeighborWidth);
      std::vector<int> rightNeighbors(this->NeighborWidth);
      LineFitting leftLine, rightLine;

      // Fill left and right neighborhoods, from central point to sides.
      // /!\ The way the neighbors are added to the vectors matters,
      // especially when computing the saliency
      for (int j = index - 1; j >= index - this->NeighborWidth; --j)
        leftNeighbors[index - 1 - j] = j;
      for (int j = index + 1; j <= index + this->NeighborWidth; ++j)
        rightNeighbors[j - index - 1] = j;

      // Fit line on the left and right neighborhoods and
      // Indicate if they are flat or not
      const bool leftFlat = leftLine.FitPCAAndCheckConsistency(scanLineCloud, leftNeighbors);
      const bool rightFlat = rightLine.FitPCAAndCheckConsistency(scanLineCloud, rightNeighbors);

      // Measurement of the depth gap
      float distLeft = 0., distRight = 0.;

      // If both neighborhoods are flat, we can compute the angle between them
      // as an approximation of the sharpness of the current point
      if (leftFlat && rightFlat)
      {
        // We check that the current point is not too far from its
        // neighborhood lines. This is because we don't want a point
        // to be considered as an angle point if it is due to gap
        distLeft = leftLine.SquaredDistanceToPoint(centralPoint);
        distRight = rightLine.SquaredDistanceToPoint(centralPoint);

        // If current point is not too far from estimated lines,
        // save the sin of angle between these two lines
        if ((distLeft < sqDistToLineThreshold) && (distRight < sqDistToLineThreshold))
          this->Angles[scanLine][index] = (leftLine.Direction.cross(rightLine.Direction)).norm();
      }

      // Here one side of the neighborhood is non flat.
      // Hence it is not worth to estimate the sharpness.
      // Only the gap will be considered here.
      // CHECK : looks strange to estimate depth gap without considering current point
      else if (!leftFlat && rightFlat)
      {
        distLeft = std::numeric_limits<float>::max();
        for (const auto& leftNeighborId: leftNeighbors)
        {
          const auto& leftNeighbor = scanLineCloud[leftNeighborId].getVector3fMap();
          distLeft = std::min(distLeft, rightLine.SquaredDistanceToPoint(leftNeighbor));
        }
        distLeft *= sqDepthDistCoeff;
      }
      else if (leftFlat && !rightFlat)
      {
        distRight = std::numeric_limits<float>::max();
        for (const auto& rightNeighborId: rightNeighbors)
        {
          const auto& rightNeighbor = scanLineCloud[rightNeighborId].getVector3fMap();
          distRight = std::min(distRight, leftLine.SquaredDistanceToPoint(rightNeighbor));
        }
        distRight *= sqDepthDistCoeff;
      }

      // No neighborhood is flat.
      // We will compute saliency of the current keypoint from its far neighbors.
      else
      {
        // Compute salient point score
        const float sqCurrDepth = centralPoint.squaredNorm();
        bool hasLeftEncounteredDepthGap = false;
        bool hasRightEncounteredDepthGap = false;

        std::vector<int> farNeighbors;
        farNeighbors.reserve(2 * this->NeighborWidth);

        // The salient point score is the distance between the current point
        // and the points that have a depth gap with the current point
        // CHECK : consider only consecutive far neighbors, starting from the central point.
        for (const auto& leftNeighborId: leftNeighbors)
        {
          // Left neighborhood depth gap computation
          if (std::abs(scanLineCloud[leftNeighborId].getVector3fMap().squaredNorm() - sqCurrDepth) > minDepthGapDist)
          {
            hasLeftEncounteredDepthGap = true;
            farNeighbors.emplace_back(leftNeighborId);
          }
          else if (hasLeftEncounteredDepthGap)
            break;
        }
        for (const auto& rightNeighborId: rightNeighbors)
        {
          // Right neigborhood depth gap computation
          if (std::abs(scanLineCloud[rightNeighborId].getVector3fMap().squaredNorm() - sqCurrDepth) > minDepthGapDist)
          {
            hasRightEncounteredDepthGap = true;
            farNeighbors.emplace_back(rightNeighborId);
          }
          else if (hasRightEncounteredDepthGap)
            break;
        }

        // If there are enough neighbors with a big depth gap,
        // we propose to compute the saliency of the current point
        // as the distance between the line that roughly fits the far neighbors
        // with a depth gap and the current point
        if (farNeighbors.size() > static_cast<unsigned int>(this->NeighborWidth))
        {
          LineFitting farNeighborsLine;
          farNeighborsLine.FitPCA(scanLineCloud, farNeighbors);
          this->Saliency[scanLine][index] = farNeighborsLine.SquaredDistanceToPoint(centralPoint);
        }
      }

      // Store max depth gap
      this->DepthGap[scanLine][index] = std::max(distLeft, distRight);
    }
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::SetKeyPointsLabels()
{
  const float sqEdgeSaliencythreshold = this->EdgeSaliencyThreshold * this->EdgeSaliencyThreshold;
  const float sqEdgeDepthGapThreshold = this->EdgeDepthGapThreshold * this->EdgeDepthGapThreshold;

  // loop over the scan lines
  #pragma omp parallel for num_threads(this->NbThreads) schedule(guided) \
          firstprivate(sqEdgeSaliencythreshold, sqEdgeDepthGapThreshold)
  for (int scanLine = 0; scanLine < static_cast<int>(this->NbLaserRings); ++scanLine)
  {
    const int Npts = this->ScanLines[scanLine]->size();

    // if the line is almost empty, skip it
    if (this->IsScanLineAlmostEmpty(Npts))
    {
      continue;
    }

    // Sort the curvature score in a decreasing order
    std::vector<size_t> sortedDepthGapIdx  = Utils::SortIdx(this->DepthGap    [scanLine], false);
    std::vector<size_t> sortedAnglesIdx    = Utils::SortIdx(this->Angles      [scanLine], false);
    std::vector<size_t> sortedSaliencyIdx  = Utils::SortIdx(this->Saliency    [scanLine], false);
    std::vector<size_t> sortedIntensityGap = Utils::SortIdx(this->IntensityGap[scanLine], false);

    // Add edge according to criterion
    auto addEdgesUsingCriterion = [this, scanLine, Npts](const std::vector<size_t>& sortedValuesIdx,
                                                         const std::vector<std::vector<float>>& values,
                                                         float threshold,
                                                         int invalidNeighborhoodSize)
    {
      for (const auto& index: sortedValuesIdx)
      {
        // Check criterion threshold
        // If criterion is not respected, break loop as indices are sorted in decreasing order.
        if (values[scanLine][index] < threshold)
          break;

        // If the point is invalid as edge, continue
        if (!this->IsPointValid[scanLine][index][Keypoint::EDGE])
          continue;

        // Else indicate that the point is an edge
        this->Label[scanLine][index].set(Keypoint::EDGE);

        // Invalid its neighbors
        const int indexBegin = std::max(0,        static_cast<int>(index - invalidNeighborhoodSize));
        const int indexEnd   = std::min(Npts - 1, static_cast<int>(index + invalidNeighborhoodSize));
        for (int j = indexBegin; j <= indexEnd; ++j)
          this->IsPointValid[scanLine][j].reset(Keypoint::EDGE);
      }
    };

    // Edges using depth gap
    addEdgesUsingCriterion(sortedDepthGapIdx, this->DepthGap, sqEdgeDepthGapThreshold, this->NeighborWidth - 1);
    // Edges using angles
    addEdgesUsingCriterion(sortedAnglesIdx, this->Angles, this->EdgeSinAngleThreshold, this->NeighborWidth);
    // Edges using saliency
    addEdgesUsingCriterion(sortedSaliencyIdx, this->Saliency, sqEdgeSaliencythreshold, this->NeighborWidth - 1);
    // Edges using intensity
    addEdgesUsingCriterion(sortedIntensityGap, this->IntensityGap, this->EdgeIntensityGapThreshold, 1);

    // Planes (using angles)
    for (int k = Npts - 1; k >= 0; --k)
    {
      size_t index = sortedAnglesIdx[k];
      const float sinAngle = this->Angles[scanLine][index];

      // thresh
      if (sinAngle > this->PlaneSinAngleThreshold)
        break;

      // if the point is invalid as plane or sinAngle value is unset, continue
      if (!this->IsPointValid[scanLine][index][Keypoint::PLANE] || sinAngle < 1e-6)
        continue;

      // else indicate that the point is a planar one
      this->Label[scanLine][index].set(Keypoint::PLANE);

      // Invalid its neighbors so that we don't have too
      // many planar keypoints in the same region. This is
      // required because of the k-nearest search + plane
      // approximation realized in the odometry part. Indeed,
      // if all the planar points are on the same scan line the
      // problem is degenerated since all the points are distributed
      // on a line.
      const int indexBegin = std::max(0,        static_cast<int>(index - 4));
      const int indexEnd   = std::min(Npts - 1, static_cast<int>(index + 4));
      for (int j = indexBegin; j <= indexEnd; ++j)
        this->IsPointValid[scanLine][j].reset(Keypoint::PLANE);
    }

    // Blobs Points
    // CHECK : why using only 1 point over 3?
    // TODO : disable blobs if not required
    for (int index = 0; index < Npts; index += 3)
    {
      if (this->IsPointValid[scanLine][index][Keypoint::BLOB])
        this->Label[scanLine][index].set(Keypoint::BLOB);
    }
  }

  for (unsigned int scanLine = 0; scanLine < this->NbLaserRings; ++scanLine)
  {
    const PointCloud& scanLineCloud = *(this->ScanLines[scanLine]);
    for (unsigned int index = 0; index < scanLineCloud.size(); ++index)
    {
      for (const auto& k : KeypointTypes)
      {
        if (this->Label[scanLine][index][k])
        {
          this->IsPointValid[scanLine][index].set(k);
          this->Keypoints[k]->push_back(scanLineCloud[index]);
        }
      }
    }
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::EstimateAzimuthalResolution()
{
  // Compute horizontal angle values between successive points
  std::vector<float> angles;
  angles.reserve(this->Scan->size());
  for (const PointCloud::Ptr& scanLine : this->ScanLines)
  {
    for (unsigned int index = 1; index < scanLine->size(); ++index)
    {
      // Compute horizontal angle between two measurements
      // WARNING: to be correct, the points need to be in the LIDAR sensor
      // coordinates system, where the sensor is spinning around Z axis.
      Eigen::Map<const Eigen::Vector2f> p1(scanLine->at(index - 1).data);
      Eigen::Map<const Eigen::Vector2f> p2(scanLine->at(index).data);
      float angle = std::abs(std::acos(p1.dot(p2) / (p1.norm() * p2.norm())));

      // Keep only angles greater than 0 to avoid dual return issues
      if (angle > 1e-4)
        angles.push_back(angle);
    }
  }

  // A minimum number of angles is needed to get a trustable estimator
  if (angles.size() < 100)
  {
    PRINT_WARNING("Not enough points to estimate azimuthal resolution");
    return;
  }

  // Estimate azimuthal resolution from these angles
  std::sort(angles.begin(), angles.end());
  unsigned int maxInliersIdx = angles.size();
  float maxAngle = Utils::Deg2Rad(5.);
  float medianAngle = 0.;
  // Iterate until only angles between direct LiDAR beam neighbors remain.
  // The max resolution angle is decreased at each iteration.
  while (maxAngle > 1.8 * medianAngle)
  {
    maxInliersIdx = std::upper_bound(angles.begin(), angles.begin() + maxInliersIdx, maxAngle) - angles.begin();
    medianAngle = angles[maxInliersIdx / 2];
    maxAngle = std::min(medianAngle * 2., maxAngle / 1.8);
  }
  this->AzimuthalResolution = medianAngle;
  std::cout << "LiDAR's azimuthal resolution estimated to " << Utils::Rad2Deg(this->AzimuthalResolution) << "°" << std::endl;
}

//-----------------------------------------------------------------------------
std::unordered_map<std::string, std::vector<float>> SpinningSensorKeypointExtractor::GetDebugArray() const
{
  auto get1DVector = [this](auto const& vector2d)
  {
    std::vector<float> v(this->Scan->size());
    std::vector<int> indexByScanLine(this->NbLaserRings, 0);
    for (unsigned int i = 0; i < this->Scan->size(); i++)
    {
      const auto& laserId = this->Scan->points[i].laser_id;
      v[i] = vector2d[laserId][indexByScanLine[laserId]];
      indexByScanLine[laserId]++;
    }
    return v;
  }; // end of lambda expression

  auto get1DVectorFromFlag = [this](auto const& vector2d, int flag)
  {
    std::vector<float> v(this->Scan->size());
    std::vector<int> indexByScanLine(this->NbLaserRings, 0);
    for (unsigned int i = 0; i < this->Scan->size(); i++)
    {
      const auto& laserId = this->Scan->points[i].laser_id;
      v[i] = vector2d[laserId][indexByScanLine[laserId]][flag];
      indexByScanLine[laserId]++;
    }
    return v;
  }; // end of lambda expression

  std::unordered_map<std::string, std::vector<float>> map;
  map["sin_angle"]      = get1DVector(this->Angles);
  map["saliency"]       = get1DVector(this->Saliency);
  map["depth_gap"]      = get1DVector(this->DepthGap);
  map["intensity_gap"]  = get1DVector(this->IntensityGap);
  map["edge_keypoint"]  = get1DVectorFromFlag(this->Label, Keypoint::EDGE);
  map["plane_keypoint"] = get1DVectorFromFlag(this->Label, Keypoint::PLANE);
  map["blob_keypoint"]  = get1DVectorFromFlag(this->Label, Keypoint::BLOB);
  map["edge_validity"]  = get1DVectorFromFlag(this->IsPointValid, Keypoint::EDGE);
  map["plane_validity"] = get1DVectorFromFlag(this->IsPointValid, Keypoint::PLANE);
  map["blob_validity"]  = get1DVectorFromFlag(this->IsPointValid, Keypoint::BLOB);
  return map;
}

} // end of LidarSlam namespace


#ifndef ESP_HPP
#define ESP_HPP

#include <libint2.hpp>
#include <vector>
#include <armadillo>
#include "Molecule.hpp"
#include "Integrals.hpp"
#include "RHF.hpp"

namespace willow { namespace qcmol {

class ESP : public RHF
{

public:
  ESP (const std::vector<libint2::Atom>& atoms,
       const libint2::BasisSet& bs,
       const Integrals& ints,
       const int qm_chg = 0,
       const bool l_print = true,
       const std::vector<QAtom>& atoms_Q = std::vector<QAtom>());

  arma::vec get_atomic_charges () { return m_qf; };

private:

  arma::vec m_qf;

  const double ang2bohr = 1.0/0.52917721067; // 2014 CODATA value

  std::vector<arma::vec3> m_grids;
  arma::vec               m_grids_val;

  void esp_grid (const std::vector<libint2::Atom>& atoms);

  arma::vec esp_fit (const std::vector<libint2::Atom>& atoms);

};

} } // namespace willow::qcmol

#endif


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [any_of]
#include <boost/simd/algorithm/any_of.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <iostream>
#include <vector>

int main()
{
  namespace bs =  boost::simd;
  std::vector<float> d{ 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};

  auto rt1 = bs::any_of( d.data(), d.data()+7, bs::is_gtz );
  std::cout <<  "rt1 " << rt1 << "\n";
  auto rf1 = bs::any_of( d.data(), d.data()+7, bs::is_ltz);
  std::cout << "rf1 " << rf1 << "\n";
  auto rt2 = bs::any_of( d.data()+1, d.data()+8);
  std::cout << "rt2 " << rt2 << "\n";
  auto rf2 = bs::any_of( d.data()+2, d.data()+9,  bs::is_nez);
  std::cout << "rf2 " << rf2 << "\n";

  return 0;
}
//! [any_of]


#include "algorithm.h"

#include <boost/heap/binomial_heap.hpp>

using namespace HybridAStar;

float aStar(Node2D& start, Node2D& goal, Node2D* nodes2D, int width, int height, CollisionDetection& configurationSpace);
void updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, int width, int height, CollisionDetection& configurationSpace);
Node3D* dubinsShot(Node3D& start, const Node3D& goal, CollisionDetection& configurationSpace);

//###################################################
//                                    NODE COMPARISON
//###################################################
/*!
   \brief A structure to sort nodes in a heap structure
*/
struct CompareNodes {
  /// Sorting 3D nodes by increasing C value - the total estimated cost
  bool operator()(const Node3D* lhs, const Node3D* rhs) const {
    return lhs->getC() > rhs->getC();
  }
  /// Sorting 2D nodes by increasing C value - the total estimated cost
  bool operator()(const Node2D* lhs, const Node2D* rhs) const {
    return lhs->getC() > rhs->getC();
  }
};

//###################################################
//                                        3D A*
//###################################################
Node3D* Algorithm::hybridAStar(Node3D& start,
                               const Node3D& goal,
                               Node3D* nodes3D,
                               Node2D* nodes2D,
                               int width,
                               int height,
                               CollisionDetection& configurationSpace) {

  // PREDECESSOR AND SUCCESSOR INDEX
  int iPred, iSucc;
  float newG;
  // Number of possible directions, 3 for forward driving and an additional 3 for reversing
  int dir = Constants::reverse ? 6 : 3;
  // Number of iterations the algorithm has run for stopping based on Constants::iterations
  int iterations = 0;

  // OPEN LIST AS BOOST IMPLEMENTATION
  typedef boost::heap::binomial_heap<Node3D*, boost::heap::compare<CompareNodes>> priorityQueue;
  priorityQueue O;

  // update h value
  updateH(start, goal, nodes2D, width, height, configurationSpace);
  // mark start as open
  start.open();
  // push on priority queue aka open list
  O.push(&start);
  iPred = start.setIdx(width, height);
  nodes3D[iPred] = start;

  // NODE POINTER
  Node3D* nPred;
  Node3D* nSucc;

  // float max = 0.f;

  // continue until O empty
  while (!O.empty()) {
    // pop node with lowest cost from priority queue
    nPred = O.top();
    // set index
    iPred = nPred->setIdx(width, height);
    iterations++;

    // _____________________________
    // LAZY DELETION of rewired node
    // if there exists a pointer this node has already been expanded
    if (nodes3D[iPred].isClosed()) {
      // pop node from the open list and start with a fresh node
      O.pop();
      continue;
    }
    // _________________
    // EXPANSION OF NODE
    else if (nodes3D[iPred].isOpen()) {
      // add node to closed list
      nodes3D[iPred].close();
      // remove node from open list
      O.pop();

      // _________
      // GOAL TEST
      if (*nPred == goal || iterations > Constants::iterations) {
        // DEBUG
        return nPred;
      }

      // ____________________
      // CONTINUE WITH SEARCH
      else {
        // _______________________
        // SEARCH WITH DUBINS SHOT
        if (Constants::dubinsShot && nPred->isInRange(goal) && nPred->getPrim() < 3) {
          nSucc = dubinsShot(*nPred, goal, configurationSpace);

          if (nSucc != nullptr && *nSucc == goal) {
            //DEBUG
            // std::cout << "max diff " << max << std::endl;
            return nSucc;
          }
        }

        // ______________________________
        // SEARCH WITH FORWARD SIMULATION
        for (int i = 0; i < dir; i++) {
          // create possible successor
          nSucc = nPred->createSuccessor(i);
          // set index of the successor
          iSucc = nSucc->setIdx(width, height);

          // ensure successor is on grid and traversable
          if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc)) {

            // ensure successor is not on closed list or it has the same index as the predecessor
            if (!nodes3D[iSucc].isClosed() || iPred == iSucc) {

              // calculate new G value
              nSucc->updateG();
              newG = nSucc->getG();

              // if successor not on open list or found a shorter way to the cell
              if (!nodes3D[iSucc].isOpen() || newG < nodes3D[iSucc].getG() || iPred == iSucc) {

                // calculate H value
                updateH(*nSucc, goal, nodes2D, width, height, configurationSpace);

                // if the successor is in the same cell but the C value is larger
                if (iPred == iSucc && nSucc->getC() > nPred->getC() + Constants::tieBreaker) {
                  delete nSucc;
                  continue;
                }
                // if successor is in the same cell and the C value is lower, set predecessor to predecessor of predecessor
                else if (iPred == iSucc && nSucc->getC() <= nPred->getC() + Constants::tieBreaker) {
                  nSucc->setPred(nPred->getPred());
                }

                if (nSucc->getPred() == nSucc) {
                  std::cout << "looping";
                }

                // put successor on open list
                nSucc->open();
                nodes3D[iSucc] = *nSucc;
                O.push(&nodes3D[iSucc]);
                delete nSucc;
              } else { delete nSucc; }
            } else { delete nSucc; }
          } else { delete nSucc; }
        }
      }
    }
  }

  if (O.empty()) {
    return nullptr;
  }

  return nullptr;
}

//###################################################
//                                        DUBINS SHOT
//###################################################

Node3D* dubinsShot(Node3D& start, const Node3D& goal, CollisionDetection& configurationSpace) {
  // start
  double q0[] = { start.getX(), start.getY(), start.getT() };
  // goal
  double q1[] = { goal.getX(), goal.getY(), goal.getT() };
  // initialize the path
  DubinsPath path;
  // calculate the path
  dubins_init(q0, q1, Constants::r, &path);

  int i = 0;
  float x = 0.f;
  float length = dubins_path_length(&path);

  Node3D* dubinsNodes = new Node3D [(int)(length / Constants::dubinsStepSize) + 1];

  while (x <  length) {
    double q[3];
    dubins_path_sample(&path, x, q);
    dubinsNodes[i].setX(q[0]);
    dubinsNodes[i].setY(q[1]);
    dubinsNodes[i].setT(Constants::normalizeHeadingRad(q[2]));

    // collision check
    if (configurationSpace.isTraversable(&dubinsNodes[i])) {

      // set the predecessor to the previous step
      if (i > 0) {
        dubinsNodes[i].setPred(&dubinsNodes[i - 1]);
      } else {
        dubinsNodes[i].setPred(&start);
      }

      if (&dubinsNodes[i] == dubinsNodes[i].getPred()) {
        std::cout << "looping shot";
      }

      x += Constants::dubinsStepSize;
      i++;
    } else {
      //      std::cout << "Dubins shot collided, discarding the path" << "\n";
      // delete all nodes
      delete [] dubinsNodes;
      return nullptr;
    }
  }

  //  std::cout << "Dubins shot connected, returning the path" << "\n";
  return &dubinsNodes[i - 1];
}

//###################################################
//                                         COST TO GO
//###################################################
void updateH(Node3D& start, const Node3D& goal, Node2D* nodes2D, int width, int height, CollisionDetection& configurationSpace) {
  float dubinsCost = 0;
  float reedsSheppCost = 0;
  float twoDCost = 0;
  float twoDoffset = 0;

  // if dubins heuristic is activated calculate the shortest path
  // constrained without obstacles
  if (Constants::dubins) {
    ompl::base::DubinsStateSpace dubinsPath(Constants::r);
    State* dbStart = (State*)dubinsPath.allocState();
    State* dbEnd = (State*)dubinsPath.allocState();
    dbStart->setXY(start.getX(), start.getY());
    dbStart->setYaw(start.getT());
    dbEnd->setXY(goal.getX(), goal.getY());
    dbEnd->setYaw(goal.getT());
    dubinsCost = dubinsPath.distance(dbStart, dbEnd);
  }

  // if reversing is active use a
  if (Constants::reverse && !Constants::dubins) {
    //    ros::Time t0 = ros::Time::now();
    ompl::base::ReedsSheppStateSpace reedsSheppPath(Constants::r);
    State* rsStart = (State*)reedsSheppPath.allocState();
    State* rsEnd = (State*)reedsSheppPath.allocState();
    rsStart->setXY(start.getX(), start.getY());
    rsStart->setYaw(start.getT());
    rsEnd->setXY(goal.getX(), goal.getY());
    rsEnd->setYaw(goal.getT());
    reedsSheppCost = reedsSheppPath.distance(rsStart, rsEnd);
  }

  // if twoD heuristic is activated determine shortest path unconstrained with obstacles
  if (Constants::twoD && !nodes2D[(int)start.getY() * width + (int)start.getX()].isDiscovered()) {
    //    ros::Time t0 = ros::Time::now();
    // create a 2d start node
    Node2D start2d(start.getX(), start.getY(), 0, 0, nullptr);
    // create a 2d goal node
    Node2D goal2d(goal.getX(), goal.getY(), 0, 0, nullptr);
    // run 2d astar and return the cost of the cheapest path for that node
    nodes2D[(int)start.getY() * width + (int)start.getX()].setG(aStar(goal2d, start2d, nodes2D, width, height, configurationSpace));
    //    ros::Time t1 = ros::Time::now();
    //    ros::Duration d(t1 - t0);
    //    std::cout << "calculated 2D Heuristic in ms: " << d * 1000 << std::endl;
  }

  if (Constants::twoD) {
    // offset for same node in cell
    twoDoffset = sqrt(((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) * ((start.getX() - (long)start.getX()) - (goal.getX() - (long)goal.getX())) +
                      ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())) * ((start.getY() - (long)start.getY()) - (goal.getY() - (long)goal.getY())));
    twoDCost = nodes2D[(int)start.getY() * width + (int)start.getX()].getG() - twoDoffset;

  }

  // return the maximum of the heuristics, making the heuristic admissable
  start.setH(std::max(reedsSheppCost, std::max(dubinsCost, twoDCost)));
}

//###################################################
//                                        2D A*
//###################################################
float aStar(Node2D& start,
            Node2D& goal,
            Node2D* nodes2D,
            int width,
            int height,
            CollisionDetection& configurationSpace) {

  // PREDECESSOR AND SUCCESSOR INDEX
  int iPred, iSucc;
  float newG;

  // reset the open and closed list
  for (int i = 0; i < width * height; ++i) {
    nodes2D[i].reset();
  }

  boost::heap::binomial_heap<Node2D*, boost::heap::compare<CompareNodes>> O;
  // update h value
  start.updateH(goal);
  // mark start as open
  start.open();
  // push on priority queue
  O.push(&start);
  iPred = start.setIdx(width);
  nodes2D[iPred] = start;

  // NODE POINTER
  Node2D* nPred;
  Node2D* nSucc;

  // continue until O empty
  while (!O.empty()) {
    // pop node with lowest cost from priority queue
    nPred = O.top();
    // set index
    iPred = nPred->setIdx(width);

    // _____________________________
    // LAZY DELETION of rewired node
    // if there exists a pointer this node has already been expanded
    if (nodes2D[iPred].isClosed()) {
      // pop node from the open list and start with a fresh node
      O.pop();
      continue;
    }
    // _________________
    // EXPANSION OF NODE
    else if (nodes2D[iPred].isOpen()) {
      // add node to closed list
      nodes2D[iPred].close();
      nodes2D[iPred].discover();

      // remove node from open list
      O.pop();

      // GOAL TEST
      if (*nPred == goal) {
        return nPred->getG();
      }
      // ____________________
      // CONTINUE WITH SEARCH
      else {
        // _______________________________
        // CREATE POSSIBLE SUCCESSOR NODES
        for (int i = 0; i < Node2D::dir; i++) {
          // create possible successor
          nSucc = nPred->createSuccessor(i);
          // set index of the successor
          iSucc = nSucc->setIdx(width);

          // ensure successor is on grid ROW MAJOR
          // ensure successor is not blocked by obstacle
          // ensure successor is not on closed list
          if (nSucc->isOnGrid(width, height) &&  configurationSpace.isTraversable(nSucc) && !nodes2D[iSucc].isClosed()) {
            // calculate new G value
            nSucc->updateG();
            newG = nSucc->getG();

            // if successor not on open list or g value lower than before put it on open list
            if (!nodes2D[iSucc].isOpen() || newG < nodes2D[iSucc].getG()) {
              // calculate the H value
              nSucc->updateH(goal);
              // put successor on open list
              nSucc->open();
              nodes2D[iSucc] = *nSucc;
              O.push(&nodes2D[iSucc]);
              delete nSucc;
            } else { delete nSucc; }
          } else { delete nSucc; }
        }
      }
    }
  }

  // return large number to guide search away
  return 1000;
}