#pragma once

#include <Eigen/Dense>
#include <cmath>
#include <robotics/common.hpp>
#include <robotics/system/nonlinear_system.hpp>
#include <vector>

namespace Robotics::Estimation {

    /**
     * @brief A class for implemeting an Extended Kalman Filter
     */
    template <int StateSize, int InputSize, int OutputSize>
    class EKF {
        static_assert(StateSize > 0);
        static_assert(InputSize > 0);
        static_assert(OutputSize > 0);

        using State = ColumnVector<StateSize>;
        using Input = ColumnVector<InputSize>;
        using Measurement = ColumnVector<OutputSize>;

        using NonlinearSystem = Robotics::Model::NonlinearSystem<StateSize, InputSize, OutputSize>;

      public:
        /**
         * @brief Creates a new Extended Kalman Filter
         * @param system nonlinear model of the system
         * @param Q state covariance matrix
         * @param R output covariance matrix
         */
        EKF(NonlinearSystem system, SquareMatrix<StateSize> Q, SquareMatrix<OutputSize> R)
            : system(system), Q(Q), R(R)
        {
        }

        /**
         * @brief Updates the state estimate
         * @param previous_estimate last estimated state
         * @param z latest measurement
         * @param u control input
         * @return the updated state estimate
         */
        State Update(State, Measurement z, Input u, double dt)
        {
            // Predicted state estimate
            system.PropagateDynamics(u, dt);
            x_predicted = system.GetState();

            // Predicted covariance estimate
            SquareMatrix<StateSize> J_F = system.GetStateJacobian(u, dt);
            P_predicted = J_F * P_estimate * J_F.transpose() + Q;

            // Update
            z_predicted = system.GetOutputMatrix() * x_predicted;
            residual = z - z_predicted;

            const Matrix<OutputSize, StateSize> J_H = system.GetOutputJacobian(u, dt);
            S = J_H * P_predicted * J_H.transpose() + R;
            K = P_predicted * J_H.transpose() * S.inverse();
            x_estimate = x_predicted + K * residual;
            P_estimate = (SquareMatrix<StateSize>::Identity() - K * J_H) * P_predicted;

            return x_estimate;
        }

      private:
        NonlinearSystem system;

        SquareMatrix<StateSize> P_predicted, P_estimate;
        SquareMatrix<OutputSize> S;
        Robotics::Matrix<StateSize, OutputSize> K;

        State x_predicted, x_estimate;
        Measurement z_predicted, residual;

        // State covariance
        const SquareMatrix<StateSize> Q;

        // Observation covariance
        const SquareMatrix<OutputSize> R;
    };

}  // namespace Robotics::Estimation


/*
 * EKF.hpp
 *
 *  Created on: 27.07.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_UKF_HPP_
#define ESTIMATORS_UKF_HPP_

#define FILL_LATER 0

#include "Eigen/Geometry"
#include <Eigen/Cholesky>
#include <OSG_Utils.hpp>

#include "../Plugins/sensor_plugin.hpp"
namespace zavi {
namespace estimator {
/**
 * Base class for all Extended Kalman Filters
 *
 * state_dim Dimension of the State
 * input_dim Dimension of the input Vector
 */
template<typename model>
class UKF: public Estimator {
protected:
	UKF(std::shared_ptr<plugin::SensorPlugin> sensor) :
			box_model(sensor), state_count(0), alignment(alignment.Identity()) {
	}
public:

	typedef model MODEL_TYPE;     //model type
	typedef typename MODEL_TYPE::template OUTER_T<double> STATE_TYPE;
	typedef typename MODEL_TYPE::template INNER_T<double> SIGMA_TYPE;
	typedef Eigen::Matrix<double, MODEL_TYPE::inner_size, MODEL_TYPE::inner_size> STATE_COV_TYPE;     //cov matrix
	typedef Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size * 2 + 1> SIGMA_POINTS_TYPE;     //< sigma points matrix type
	typedef Eigen::Matrix<double, MODEL_TYPE::inner_size, MODEL_TYPE::inner_size * 2 + 1> SIGMA_SIGMA_POINTS_TYPE;     //< occurs when the difference of SIGMA_POINTS with the state is drawn

	virtual ~UKF() {
		//smoothAllEstimates();

	}

	STATE_TYPE boxPlus(const STATE_TYPE & state, const SIGMA_TYPE & delta) {
		return box_model.template boxPlus<double>(state, delta);
	}

	SIGMA_TYPE boxMinus(const STATE_TYPE & a, const STATE_TYPE & b) {
		return box_model.template boxMinus<double>(a, b);
	}

	STATE_TYPE stateTransitionFunction(const STATE_TYPE & state, const double time_diff) {
		return box_model.template stateTransitionFunction<double>(state, time_diff);
	}

	/**
	 * Returns the sigma points of the state
	 *
	 *
	 * @param state the state to get the sigma points off
	 * @param cov the covariance of the state
	 * @return a eigen matrix with the sigma points
	 */

	SIGMA_POINTS_TYPE getSigmaPoints(const STATE_TYPE & state, const STATE_COV_TYPE & cov) {
		STATE_COV_TYPE cholesky = cov.llt().matrixL();
		SIGMA_POINTS_TYPE sigma_points = SIGMA_POINTS_TYPE::Zero();
		STATE_COV_TYPE neg_cholesky = STATE_COV_TYPE::Zero() - cholesky;
		Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size> negResult, posResult;
		negResult = negResult.Zero();
		posResult = posResult.Zero();
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			posResult.col(i) = boxPlus(state, cholesky.col(i));
		}
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			negResult.col(i) = boxPlus(state, neg_cholesky.col(i));
		}
		sigma_points << state, posResult, negResult;

		return sigma_points;
	}

	/**
	 * Simple wrapper to call getSigmaPoints without arguments
	 * @return sigma points of state_vector and covariance
	 */
	virtual SIGMA_POINTS_TYPE getSigmaPoints() {
		return getSigmaPoints(state_vector, covariance);
	}

	/**
	 * Calculates the mean of sigma points
	 * @param sigma_points  the matrix with all sigma points
	 * @param epsilon the stopping criteria
	 * @param max_iterations max iterations of convergence
	 * @return the mean of sigma points
	 */
	STATE_TYPE meanOfSigmaPoints(const SIGMA_POINTS_TYPE &sigma_points, double epsilon = 1e-8,
			int max_iterations = 30) {
		STATE_TYPE mean = sigma_points.col(0);
		STATE_TYPE old_mean = sigma_points.col(0);
		SIGMA_TYPE diff_sum = SIGMA_TYPE::Zero();
		int iterations = 0;
		do {
			iterations++;
			old_mean = mean;
			diff_sum = diff_sum.Zero();
			for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
				diff_sum += boxMinus(mean, sigma_points.col(i));
			}
			diff_sum /= 2. * MODEL_TYPE::inner_size + 1.;
			mean = boxPlus(mean, diff_sum);
		} while (iterations <= max_iterations && boxMinus(mean, old_mean).norm() > epsilon);
		if (iterations > max_iterations)
			printf("Warning: stopped due to excess of iterations");
		return mean;
	}

	/**
	 * Calculates the mean of sigma points
	 * @param sigma_points  the matrix with all sigma points
	 * @param epsilon the stopping criteria
	 * @param max_iterations max iterations of convergence
	 * @return the mean of sigma points
	 */
	template<int measure_dim, int measure_inner_dim>
	Eigen::Matrix<double, measure_dim, 1> meanOfSigmaPoints(
			const Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> &sigma_points,
			Eigen::Matrix<double, measure_dim, 1> (*boxplus_m)(const Eigen::Matrix<double, measure_dim, 1> & state,
					const Eigen::Matrix<double, measure_inner_dim, 1> & delta),
			Eigen::Matrix<double, measure_inner_dim, 1> (*boxminus_m)(const Eigen::Matrix<double, measure_dim, 1> & a,
					const Eigen::Matrix<double, measure_dim, 1> & b), double epsilon = 1e-6, int max_iterations = 30) {

		Eigen::Matrix<double, measure_dim, 1> mean = sigma_points.col(0);
		Eigen::Matrix<double, measure_dim, 1> old_mean = sigma_points.col(0);
		Eigen::Matrix<double, measure_inner_dim, 1> diff_sum = Eigen::Matrix<double, measure_inner_dim, 1>::Zero();
		int iterations = 0;
		do {
			iterations++;
			old_mean = mean;
			diff_sum = diff_sum.Zero();
			for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
				diff_sum += boxminus_m(mean, sigma_points.col(i));
			}
			diff_sum /= 2. * MODEL_TYPE::inner_size + 1.;
			mean = boxplus_m(mean, diff_sum);
		} while (iterations <= max_iterations && boxminus_m(mean, old_mean).norm() > epsilon);
		if (iterations > max_iterations)
			printf("Warning: stopped due to excess of iterations");
		return mean;
	}
	/**
	 * Saves all releveant states for smoothing
	 * @param input the input u
	 * @param time_diff time since last call
	 */
	void saveStatesForSmoothing(double time_diff) {
		state_count++;
		past_states.push_back(state_vector);
		past_states_smoothed.push_back(state_vector);
		past_covs.push_back(covariance);
		past_timediffs.push_back(time_diff);
	}
	/**
	 * Does  dynamic step in EKF
	 *
	 * @param input the input u
	 * @param time_diff time since last call
	 */
	void dynamicStep(double time_diff) {
		SIGMA_POINTS_TYPE sigma_points = getSigmaPoints();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			sigma_points.col(i) = stateTransitionFunction(sigma_points.col(i), time_diff);
		}
		state_vector = meanOfSigmaPoints(sigma_points);
		//printf(state_vector);
		//printf(" ");
		SIGMA_SIGMA_POINTS_TYPE result = SIGMA_SIGMA_POINTS_TYPE::Zero();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points.col(i));
		}
		covariance = 0.5 * (result * result.transpose()) + processNoise(time_diff);
		//printf("dynamic");
		//printf(state_vector.block(3, 0, 3, 1).norm());
	}
	/**
	 * Does  a  measurement update in UKF
	 * @param measurement the measurement
	 * @param time_diff time since last call
	 * @param measure_function function to map a state to a predicted measurement
	 * @param noise_function gives the measurement noise with time_diff
	 * @param boxplus_m boxplus for measurement
	 * @param boxminus_m boxminus for measurement
	 * measure dim is the dimension of the measurement vector
	 */
	template<int measure_inner_dim, typename functor, int measure_dim>
	void measurementStepManifold(const Eigen::Matrix<double, measure_dim, 1> & measurement, double time_diff,
			const functor & measure_function, const Eigen::Matrix<double, measure_inner_dim, measure_inner_dim> & noise,
			Eigen::Matrix<double, measure_dim, 1> (*boxplus_m)(const Eigen::Matrix<double, measure_dim, 1> & state,
					const Eigen::Matrix<double, measure_inner_dim, 1> & delta),
			Eigen::Matrix<double, measure_inner_dim, 1> (*boxminus_m)(const Eigen::Matrix<double, measure_dim, 1> & a,
					const Eigen::Matrix<double, measure_dim, 1> & b), void *prior = NULL) {
		SIGMA_POINTS_TYPE sigma_points = getSigmaPoints();
		Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> expected_zs;
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			expected_zs.col(i) = measure_function(STATE_TYPE(sigma_points.col(i)), alignment, prior);
		}
		Eigen::Matrix<double, measure_dim, 1> mean_z = meanOfSigmaPoints(expected_zs, boxplus_m, boxminus_m);     //< expected measurement mean
		Eigen::Matrix<double, measure_inner_dim, MODEL_TYPE::inner_size * 2 + 1> diff_z;
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			diff_z.col(i) = boxminus_m(mean_z, expected_zs.col(i));
		}
		Eigen::Matrix<double, measure_inner_dim, measure_inner_dim> sigma_z = 0.5 * (diff_z * diff_z.transpose())
				+ noise;
		SIGMA_SIGMA_POINTS_TYPE result = SIGMA_SIGMA_POINTS_TYPE::Zero();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points.col(i));
		}
		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_inner_dim> sigma_xz = 0.5 * (result * diff_z.transpose());
		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_inner_dim> kalman_gain = sigma_xz * sigma_z.inverse();
		Eigen::Matrix<double, MODEL_TYPE::inner_size, 1> delta = kalman_gain * boxminus_m(mean_z, measurement);
		STATE_COV_TYPE sigma_t = covariance - (kalman_gain * sigma_z * kalman_gain.transpose());
		SIGMA_POINTS_TYPE sigma_points_second = SIGMA_POINTS_TYPE::Zero();
		STATE_COV_TYPE cholesky = sigma_t.llt().matrixL();
		Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size> negResult, posResult;
		negResult = negResult.Zero();
		posResult = posResult.Zero();
		STATE_COV_TYPE neg_cholesky = STATE_COV_TYPE::Zero() - cholesky;
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			posResult.col(i) = boxPlus(state_vector,
					box_model.template boxPlusInnerSpace<double>(delta, cholesky.col(i)));
		}
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			negResult.col(i) = boxPlus(state_vector,
					box_model.template boxPlusInnerSpace<double>(delta, neg_cholesky.col(i)));
		}
		sigma_points_second << boxPlus(state_vector, delta), posResult, negResult;

		state_vector = meanOfSigmaPoints(sigma_points_second);
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points_second.col(i));
		}
		covariance = 0.5 * (result * result.transpose());

	}
	/**
	 * Does  a  measurement update in UKF
	 * @param measurement the measurement
	 * @param time_diff time since last call
	 * @param measure_function function to map a state to a predicted measurement
	 * @param noise_function gives the measurement noise with time_diff
	 * measure dim is the dimension of the measurement vector
	 */
	template<typename functor, int measure_dim>
	void measurementStep(const Eigen::Matrix<double, measure_dim, 1> & measurement, double time_diff,
			const functor & measure_function, const Eigen::Matrix<double, measure_dim, measure_dim> & noise,
			void * prior = NULL) {

		SIGMA_POINTS_TYPE sigma_points = getSigmaPoints();
		Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> expected_zs;
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			expected_zs.col(i) = measure_function(STATE_TYPE(sigma_points.col(i)), alignment, prior);
		}
		Eigen::Matrix<double, measure_dim, 1> mean_z = expected_zs.rowwise().mean();     //< expected measurement mean
		Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> diff_z = expected_zs.colwise() - mean_z;
		Eigen::Matrix<double, measure_dim, measure_dim> sigma_z = 0.5 * (diff_z * diff_z.transpose()) + noise;

		SIGMA_SIGMA_POINTS_TYPE result = SIGMA_SIGMA_POINTS_TYPE::Zero();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points.col(i));
		}
		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_dim> sigma_xz = 0.5 * (result * diff_z.transpose());

		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_dim> kalman_gain = sigma_xz * sigma_z.inverse();

		state_vector = boxPlus(state_vector, kalman_gain * (measurement - mean_z));

		covariance = covariance - kalman_gain * sigma_xz.transpose();
	}
	/**
	 * wrapper to call measurementstep with different arguments
	 */
	template<int measure_dim>
	struct MeasurementWrapper {
		Eigen::Matrix<double, measure_dim, 1> (*function)(const STATE_TYPE &, void *);
		MeasurementWrapper(Eigen::Matrix<double, measure_dim, 1> (*function)(const STATE_TYPE &, void *)) :
				function(function) {

		}
		Eigen::Matrix<double, measure_dim, 1> operator()(const STATE_TYPE & state,
				const Eigen::Matrix<double, 4, 4> & alignment, void *prior) const {
			return function(state, prior);
		}

	};
	template<int measure_dim>
	void measurementStep(const Eigen::Matrix<double, measure_dim, 1> & measurement, double time_diff,
			Eigen::Matrix<double, measure_dim, 1> (*measure_function)(const STATE_TYPE &, void * prior),
			const Eigen::Matrix<double, measure_dim, measure_dim> & noise, void * prior = NULL) {
		measurementStep(measurement, time_diff, MeasurementWrapper<measure_dim>(measure_function), noise, prior);
	}

	/**
	 * Smooth previous estimates with new knowledge
	 *
	 * This is taken from:
	 *
	 * Unscented Rauch–Tung–Striebel Smoother by Simo S"arkk"a
	 *
	 * @param k_length the amount of steps to smooth back
	 * @param k_start the starting index for the smoothing
	 */
	void smoothEstimates(unsigned int k_length, unsigned int k_start) {
		if (k_start > state_count - 1) {
			LOG(ERROR)<<"Trying to smooth from a non existent state";
			return;
		}

		if (k_length > k_start) {
			LOG(ERROR)<< "Trying to smooth more states than are before k_start";
			return;
		}
		for (unsigned int k = k_start; k > k_start - k_length; k--) {
			SIGMA_POINTS_TYPE sigma_points = getSigmaPoints(past_states_smoothed[k], past_covs[k]);
			SIGMA_POINTS_TYPE sigma_points_plus;
			for (int i = 0; i < MODEL_TYPE::inner_size * 2 + 1; i++) {
				sigma_points_plus.col(i) = stateTransitionFunction(sigma_points.col(i), past_timediffs[k]);
			}

			STATE_TYPE mean = meanOfSigmaPoints(sigma_points_plus);
			SIGMA_SIGMA_POINTS_TYPE result_plus;
			for (int i = 0; i < MODEL_TYPE::inner_size * 2 + 1; i++) {
				result_plus.col(i) = boxMinus(mean, sigma_points_plus.col(i));
			}

			SIGMA_SIGMA_POINTS_TYPE result;
			STATE_COV_TYPE cov = 0.5*(result_plus * result_plus.transpose())+ processNoise(past_timediffs[k]);
			for (int i = 0; i < MODEL_TYPE::inner_size * 2 + 1; i++) {
				result.col(i) = boxMinus(past_states_smoothed[k], sigma_points.col(i));
			}
			STATE_COV_TYPE c_k_plus =0.5* result * result_plus.transpose();
			STATE_COV_TYPE d_k = c_k_plus * cov.inverse();
			//past_states_smoothed[k] = boxPlus(past_states_smoothed[k],
			//		d_k * boxMinus(mean, past_states_smoothed[k + 1]));
			//past_covs[k] = past_covs[k] + d_k * (past_covs[k + 1] - cov) * d_k.transpose();

			//from here the second sigma propagation applies
			SIGMA_TYPE delta=d_k * boxMinus(mean, past_states_smoothed[k + 1]);
			STATE_COV_TYPE pst_cov_k=past_covs[k] + d_k * (past_covs[k + 1] - cov) * d_k.transpose();

			SIGMA_POINTS_TYPE sigma_points_second = SIGMA_POINTS_TYPE::Zero();
			STATE_COV_TYPE cholesky = pst_cov_k.llt().matrixL();
			Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size> negResult, posResult;
			negResult = negResult.Zero();
			posResult = posResult.Zero();
			STATE_COV_TYPE neg_cholesky = STATE_COV_TYPE::Zero() - cholesky;
			for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
				posResult.col(i) = boxPlus(past_states_smoothed[k],
						box_model.template boxPlusInnerSpace<double>(delta, cholesky.col(i)));
			}
			for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
				negResult.col(i) = boxPlus(past_states_smoothed[k],
						box_model.template boxPlusInnerSpace<double>(delta, neg_cholesky.col(i)));
			}
			sigma_points_second << boxPlus(past_states_smoothed[k], delta), posResult, negResult;

			past_states_smoothed[k] = meanOfSigmaPoints(sigma_points_second);
			for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
				result.col(i) = boxMinus(past_states_smoothed[k], sigma_points_second.col(i));
			}
			past_covs[k] = 0.5 * (result * result.transpose());
		}
	}
	/**
	 * Perform smoothing from the newest estimate
	 * @param k_length the amount of estimates to smooth
	 */
	void smoothEstimates(unsigned int k_length) {

		smoothEstimates(k_length, state_count - 2);

	}
	/**
	 * Perform smoothing from the newest estimate for all estimates
	 */
	void smoothAllEstimates() {
		smoothEstimates(state_count - 2);
	}

	static void smoothCallback(plugin::SensorPlugin * plug, void* estimator, double time) {
		UKF * esti = static_cast<UKF *>(estimator);
		esti->smoothAllEstimates();
	}

	/**
	 * Sets the start estimat \hat(x) (0)
	 * @param start_state the starting state vector
	 * @param start_cov  the starting covariance
	 */
	virtual inline void setStart(const STATE_TYPE & start_state, const STATE_COV_TYPE &start_cov) {
		state_vector = start_state;
		covariance = start_cov;
	}

	/**
	 * Gives the dimension of the state
	 * @return Dimension of the state vector
	 */
	static constexpr int getStateDim() {
		return MODEL_TYPE::outer_size;
	}

	virtual inline STATE_COV_TYPE processNoise(const double time_diff) {
		STATE_COV_TYPE matrix = box_model.getStateSTD(time_diff).asDiagonal();

		//zavi::printf(matrix);
		return matrix;
	}

	inline MODEL_TYPE getBoxModel() {
		return box_model;
	}
	inline STATE_TYPE getStateVector() {
		return state_vector;
	}
	inline STATE_COV_TYPE getCov() {
		return covariance;
	}

	std::vector<STATE_TYPE> & getSmoothedStates() {
		return past_states_smoothed;
	}

	void setAlignment(const Eigen::Matrix4d & alignment) {
		this->alignment=alignment;
	}

protected:
	MODEL_TYPE box_model;     // The Model type
	STATE_TYPE state_vector;//< the current estimated state x
	STATE_COV_TYPE covariance;//< the estimated cov(x)
	std::vector<STATE_TYPE> past_states;//< all past states
	std::vector<STATE_TYPE> past_states_smoothed;//< all past states
	//std::vector<INPUT_TYPE> past_inputs;//< all past inputs
	std::vector<double> past_timediffs;//< all past time_diffs

	std::vector<STATE_COV_TYPE> past_covs;// < all past covariance matrices
	unsigned int state_count;//< the current state index
	Eigen::Matrix<double, 4, 4> alignment;
};

}
/* namespace estimator */
}
/* namespace zavi */

#endif /* ESTIMATORS_UKF_HPP_ */


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Alpha_shape_vertex_base_2.h>
#include <CGAL/Alpha_shape_face_base_2.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/algorithm.h>
#include <CGAL/assertions.h>
#include <fstream>
#include <iostream>
#include <list>
#include <vector>
#include <algorithm>
#include <unordered_set>
#include <boost/program_options.hpp>
#include <boost/functional/hash.hpp>

namespace po = boost::program_options;

typedef CGAL::Exact_predicates_inexact_constructions_kernel  K;
typedef K::FT                                                FT;
typedef K::Point_2                                           Point;
typedef K::Segment_2                                         Segment;
typedef CGAL::Alpha_shape_vertex_base_2<K>                   Vb;
typedef CGAL::Alpha_shape_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>          Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                Triangulation_2;
using Edge = Triangulation_2::Edge;
using Vertex = Triangulation_2::Vertex_handle;
typedef CGAL::Alpha_shape_2<Triangulation_2>                 Alpha_shape_2;
typedef Alpha_shape_2::Alpha_shape_edges_iterator            Alpha_shape_edges_iterator;

template <class OutputIterator>
void alpha_edges( const Alpha_shape_2& A, OutputIterator out)
{
  Alpha_shape_edges_iterator it = A.alpha_shape_edges_begin(),
                             end = A.alpha_shape_edges_end();
  for( ; it!=end; ++it) {
    Edge e = *it;
    auto r0 = A.classify(e.first);
    auto r = A.classify(e);

    *out++ = A.segment(*it);
  }

  auto vit = A.alpha_shape_vertices_begin(),
       vend = A.alpha_shape_vertices_end();
  for( ; vit!=vend; ++vit) {
    Vertex v = *vit;
    auto r = A.classify(v);
  }
}

template <class OutputIterator>
void alpha_verts( const Alpha_shape_2& A, OutputIterator out)
{
  auto vit = A.alpha_shape_vertices_begin(),
       vend = A.alpha_shape_vertices_end();
  for( ; vit!=vend; ++vit) {
    Vertex v = *vit;
    *(++out) = v->point();
  }
}

template <class OutputIterator>
bool file_input(const std::string& in, OutputIterator out)
{
  std::ifstream is(in, std::ios::in);
  if(is.fail())
  {
    std::cerr << "unable to open file for input" << std::endl;
    return false;
  }
  int n;
  is >> n;
  std::cout << "Reading " << n << " points from file" << std::endl;
  CGAL::copy_n(std::istream_iterator<Point>(is), n, out);
  return true;
}

bool save_output(const std::string& out,
                 const std::vector<Segment>& segments,
                 const std::vector<Point>& verts)
{
  std::ofstream os(out, std::ios::out);
  if(os.fail())
  {
    std::cerr << "unable to open file for output" << std::endl;
    return false;
  }

  os << segments.size() << std::endl;
  for (const auto& seg: segments) {
      os << std::fixed << std::setprecision(6) << seg << std::endl;
  }

  os << verts.size() << std::endl;
  for (const auto& v: verts) {
      os << std::fixed << std::setprecision(6) << v << std::endl;
  }

  return true;
}

struct Input {
    std::string in;
    std::string out;
    double alpha;
};

Input parse_input(int argc, char * argv[]) {
    po::options_description desc("Allowed options");
    desc.add_options()
        ("in", po::value<std::string>()->required(), "input points")
        ("out", po::value<std::string>()->required(), "output edges and verts")
        ("alpha", po::value<double>()->required(), "alpha");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    Input input;
    input.in = vm["in"].as<std::string>();
    input.out = vm["out"].as<std::string>();
    input.alpha = vm["alpha"].as<double>();

    if (input.alpha <= 0)
        throw std::invalid_argument("alpha should be > 0");
    return input;
}

std::vector<Point> filter_verts(const std::vector<Segment>& segments,
                                std::vector<Point>&& verts) {
    struct HashVert {
        size_t operator()(const Point& p) const {
            size_t res = 0;
            boost::hash_combine(res, p.x());
            boost::hash_combine(res, p.y());
            return res;
        }
    };

    std::unordered_set<Point, HashVert> seg_points;
    for (const auto& seg: segments) {
        seg_points.insert(seg.source());
        seg_points.insert(seg.target());
    }

    verts.erase(
        std::remove_if(verts.begin(), verts.end(), [&seg_points](const auto& p) {
            return seg_points.count(p) > 0;
        }),
        verts.end()
    );

    return std::move(verts);
}

// Reads a list of points and returns a list of segments
// corresponding to the Alpha shape.
int main(int argc, char * argv[])
{
  const auto input = parse_input(argc, argv);
  std::list<Point> points;
  if(! file_input(input.in, std::back_inserter(points)))
    return -1;
  Alpha_shape_2 A(points.begin(), points.end(),
                  FT(input.alpha),
                  Alpha_shape_2::GENERAL);

  std::cout<< " Components for alpha " << input.alpha << " " << A.number_of_solid_components() << std::endl;

  std::vector<Segment> segments;
  std::vector<Point> res_points;
  alpha_edges(A, std::back_inserter(segments));
  alpha_verts(A, std::back_inserter(res_points));
  auto filtered_points = filter_verts(segments, std::move(res_points));
  save_output(input.out, segments, filtered_points);

  return 0;
}


/*! \file mesh_generator3D.hpp
\brief Set of functions to generate points.
\author Elad Steinberg
*/
#ifndef MESHGENERATOR3D_HPP
#define MESHGENERATOR3D_HPP 1

#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif // _MSC_VER
#include <vector>
#include <cmath>
#include "../3D/GeometryCommon/Voronoi3D.hpp"
#include <algorithm>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

/*! \brief Generates a cartesian mesh
\param nx Number of points along the x axis
\param ny Number of points along the y axis
\param nz Number of points along the z axis
\param lower_left Lower left point
\param upper_right Upper right point
\return Set of three dimensional points
*/
vector<Vector3D> CartesianMesh(std::size_t nx, std::size_t ny, std::size_t nz, Vector3D const& lower_left,
	Vector3D const& upper_right);

/*!
\brief Generates a random grid with uniform point density and a constant seed
\param PointNum The number of points.
\param ll The lower left point of the domain
\param ur The upper right point of the domain
\return List of three dimensional points
*/
vector<Vector3D> RandRectangular(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur,Voronoi3D const* tproc = 0);

vector<Vector3D> RandRectangular(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, boost::mt19937_64 &gen);

vector<Vector3D> RandSphereR(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, double Rmin, double Rmax,
	Vector3D center = Vector3D(),Voronoi3D const* tproc = 0);

vector<Vector3D> RandSphereR2(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur,double Rmin,double Rmax
	, Vector3D center = Vector3D(), Voronoi3D const* tproc = 0);

vector<Vector3D> RandSphereR1(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, double Rmin, double Rmax,
	Vector3D center = Vector3D(),Voronoi3D const* tproc = 0);

vector<Vector3D> RandSphereRa(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, double Rmin, double Rmax,double a, Vector3D const& center,
	Voronoi3D const* tproc = 0);

#ifdef RICH_MPI
/*!
\brief Generates a random grid with uniform point density and a constant seed
\param PointNum The total number of points to be in all cpus combined.
\param tproc The tessellation of the processors
\return List of three dimensional points
*/
vector<Vector3D> RandPointsMPI(Voronoi3D const& tproc, size_t PointNum);
#endif

#endif //MESHGENERATOR3D_HPP


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <math.h>
#include <limits>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include "modules/world/opendrive/plan_view.hpp"
#include "modules/world/opendrive/lane.hpp"
#include "modules/world/opendrive/odrSpiral.hpp"

namespace modules {
namespace world {
namespace opendrive {

namespace bg = boost::geometry;

bool PlanView::add_line(geometry::Point2d start_point, float heading, float length) {
  //! straight line
  reference_line_.add_point(start_point);
  geometry::Point2d end_point(bg::get<0>(start_point) + length * cos(heading), bg::get<1>(start_point) + length * sin(heading));
  reference_line_.add_point(end_point);

  //! calculate overall length
  length_ = bg::length(reference_line_.obj_);
  return true;
}

bool PlanView::add_spiral(geometry::Point2d start_point, float heading, float length, float curvature_start, float curvature_end, float s_inc) {
  double x = bg::get<0>(start_point), y = bg::get<1>(start_point), t = heading, cDot = (curvature_end - curvature_start) / length;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);

  double s = 0.0;
  for (; s < length; s += s_inc) {
    odrSpiral(s, x_old, y_old, cDot, curvature_start, heading, &x, &y, &t);
    reference_line_.add_point(geometry::Point2d(x, y));
  }

  // fill last point if increment does not match
  double delta_s = fabs(length - s);
  if (delta_s > 0.0) {
    odrSpiral(length, x_old, y_old, cDot, curvature_start, heading, &x, &y, &t);
    reference_line_.add_point(geometry::Point2d(x, y));
  }

  length_ = bg::length(reference_line_.obj_);
  return true;
}

void PlanView::calc_arc_position(const float s, float initial_heading, float curvature, float &dx, float &dy) {
  initial_heading = fmod(initial_heading, 2 * M_PI);
  float hdg = initial_heading - M_PI / 2;

  float a = 2 / curvature * sin(s * curvature / 2);
  float alpha = (M_PI - s * curvature) / 2 - hdg;

  dx = -1 * a * cos(alpha);
  dy = a * sin(alpha);

  // tangent = initial_heading + s * initial_curvature;
}

bool PlanView::add_arc(geometry::Point2d start_point, float heading, float length, float curvature, float s_inc) {
  // add_spiral(start_point, heading, length, curvature, curvature, s_inc);

  float dx, dy;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);
  double s = 0.0;
  for (; s < length; s += s_inc) {
    calc_arc_position(s, heading, curvature, dx, dy);
    reference_line_.add_point(geometry::Point2d(x_old + dx, y_old + dy));
  }

  // fill last point if increment does not match
  double delta_s = fabs(length - s);
  if (delta_s >= 0.0){
    calc_arc_position(length, heading, curvature, dx, dy);
    reference_line_.add_point(geometry::Point2d(x_old + dx, y_old + dy));
  }

  return true;
}

geometry::Line PlanView::create_line(int id, LaneWidth lane_width, float s_inc) {
  float s_start = lane_width.s_start;
  float s_end = lane_width.s_end;
  LaneOffset off = lane_width.off;

  float s = s_start;
  float scale = 0.0f;
  geometry::Line tmp_line;
  geometry::Point2d normal(0.0f, 0.0f);
  int sign = id > 0 ? -1 : 1;

  // TODO(fortiss): check if sampling does work with relative s, probably not
  if (off.b != 0.0f || off.c != 0.0f || off.d != 0.0f || (lane_width.s_end - lane_width.s_start) != 1.0) {
    for (; s < s_end; s += s_inc) {
      geometry::Point2d point = get_point_at_s(reference_line_, s);
      normal = get_normal_at_s(reference_line_, s);
      scale = -sign * polynom(s, off.a, off.b, off.c, off.d);
      tmp_line.add_point(geometry::Point2d(bg::get<0>(point) + scale * bg::get<0>(normal),
                                  bg::get<1>(point) + scale * bg::get<1>(normal)));
    }

    // fill last point if increment does not match
    double delta_s = fabs(s_end-s);
    if(delta_s>0.0){
      geometry::Point2d point = get_point_at_s(reference_line_, s_end);
      normal = get_normal_at_s(reference_line_, s_end);
      scale = -sign * polynom(s_end, off.a, off.b, off.c, off.d);
      tmp_line.add_point(geometry::Point2d(bg::get<0>(point) + scale * bg::get<0>(normal),
                                  bg::get<1>(point) + scale * bg::get<1>(normal)));
    }
  } else {
      for (uint32_t i = 0; i < reference_line_.obj_.size() - 1; i++) {
        normal = get_normal_at_s(reference_line_, reference_line_.s_[i]);
        scale = -sign * polynom(s, off.a, off.b, off.c, off.d);
        tmp_line.add_point(geometry::Point2d(bg::get<0>(reference_line_.obj_[i]) + scale * bg::get<0>(normal),
                                  bg::get<1>(reference_line_.obj_[i]) + scale * bg::get<1>(normal)));
        s += geometry::distance(reference_line_.obj_[i + 1], reference_line_.obj_[i]);
      }
      // add last point
      normal = get_normal_at_s(reference_line_, reference_line_.s_[reference_line_.obj_.size() - 1]);
      int size = reference_line_.obj_.size() - 1;
      scale = -sign * polynom(length_, off.a, off.b, off.c, off.d);
      tmp_line.add_point(geometry::Point2d(bg::get<0>(reference_line_.obj_[size]) + scale * bg::get<0>(normal),
                                bg::get<1>(reference_line_.obj_[size]) + scale * bg::get<1>(normal)));
  }

  return tmp_line;
}

//! TODO: this function needs to resive a vector of Struct {s_start, s_end, off}
LanePtr PlanView::create_lane(LanePosition lane_position, LaneWidths lane_widths, float s_inc) {
  std::shared_ptr<Lane> ret_lane(new Lane(lane_position));
  if (lane_widths.size() > 1) {
    assert("Not supported");
  }
  for (LaneWidth lane_width : lane_widths) {
    geometry::Line tmp_line = create_line(lane_position, lane_width, s_inc);
    ret_lane->set_line(tmp_line);
  }

  // ret_lane->ComputeCenterLine();
  return ret_lane;
}

}  // namespace opendrive
}  // namespace world
}  // namespace modules


#include <iostream>
#include <unistd.h>
#include <Eigen/Dense>
#include "OrthonormalHermite.h"
#include "NelderMead.h"
#include "VariableProjection.h"

using namespace std;

int main()
{
    APPRSDK::VariableProjection<double> approximator;
    APPRSDK::OrthonormalHermite<double> hermiteSys(100, 10);

    Eigen::RowVectorXd inputParameters;
    inputParameters.resize(2);
    inputParameters(0) = 0.7;
    inputParameters(1) = 50;

    Eigen::RowVectorXd lb;
    lb.resize(2);
    lb(0) = 0.01;
    lb(1) = -1000;

    Eigen::RowVectorXd ub;
    ub.resize(2);
    ub(0) = 1000;
    ub(1) = 1000;

    APPRSDK::AvailableOptimizers optId = APPRSDK::AvailableOptimizers::NM;

    approximator.SetNonLinParams(inputParameters);
    approximator.SetMaxErrorForOptimisation(0.01);
    approximator.SetMaxIterationForOptimisation(100);
    approximator.SetFunctionSystem(&hermiteSys);
    approximator.SelectOptimiser(optId, true);
	approximator.SetBoundaries(lb, ub);
    approximator.SetSignal(hermiteSys.GetFunctionSystem().col(4).transpose());

    approximator.Varpro();

	cout<<"Signal: "<<approximator.GetSignal().transpose()<<endl;
	cout<<"Approximaton: "<<approximator.GetApproximation().transpose()<<endl;
	cout<<"Coefficients: "<<approximator.GetLinearParameters().transpose()<<endl;
	cout<<"Dilatation & Translation: "<<approximator.GetNonLinearParameters()<<endl;
    cout<<"Iterations: "<<approximator.GetIterations()<<endl;
    cout<<"Final error: "<<approximator.GetError()<<endl;

    return 0;
}


/* This file is part of PyMesh. Copyright (c) 2017 by Qingnan Zhou */
#include "VoxelDihedralAngleAttribute.h"

#include <cmath>
#include <Eigen/Core>

#include <Mesh.h>
#include <Core/Exception.h>

using namespace PyMesh;

namespace VoxelDihedralAngleAttributeHelper {
    Float angle(const Eigen::Ref<Vector3F>& n1,
            const Eigen::Ref<Vector3F>& n2) {
        return atan2(n1.cross(n2).norm(), n1.dot(n2));
    }

    Vector3F compute_normal(
            const Vector3F& v1,
            const Vector3F& v2,
            const Vector3F& v3) {
        return (v2 - v1).cross(v3 - v1);
    }
}

using namespace VoxelDihedralAngleAttributeHelper;

void VoxelDihedralAngleAttribute::compute_from_mesh(Mesh& mesh) {
    const size_t dim = mesh.get_dim();
    const size_t num_voxels = mesh.get_num_voxels();
    const size_t vertex_per_voxel = mesh.get_vertex_per_voxel();
    if (dim != 3) {
        throw RuntimeError("Voxel dihedral anlge computation is for 3D only.");
    }
    if (num_voxels > 0 && vertex_per_voxel != 4) {
        throw NotImplementedError(
                "Voxel dihedral angle computation only support tet for now.");
    }

    const auto& vertices = mesh.get_vertices();
    const auto& voxels = mesh.get_voxels();
    VectorF& dihedral_angles = m_values;
    dihedral_angles.resize(num_voxels * 6);

    for (size_t i=0; i<num_voxels; i++) {
        Vector4I v = voxels.segment<4>(i*4);
        Vector3F v0 = vertices.segment<3>(v[0]*3);
        Vector3F v1 = vertices.segment<3>(v[1]*3);
        Vector3F v2 = vertices.segment<3>(v[2]*3);
        Vector3F v3 = vertices.segment<3>(v[3]*3);

        Vector3F n0 = compute_normal(v1, v2, v3);
        Vector3F n1 = compute_normal(v0, v3, v2);
        Vector3F n2 = compute_normal(v0, v1, v3);
        Vector3F n3 = compute_normal(v0, v2, v1);

        dihedral_angles[i*6  ] = M_PI - angle(n2, n3);
        dihedral_angles[i*6+1] = M_PI - angle(n0, n3);
        dihedral_angles[i*6+2] = M_PI - angle(n1, n3);
        dihedral_angles[i*6+3] = M_PI - angle(n1, n2);
        dihedral_angles[i*6+4] = M_PI - angle(n0, n1);
        dihedral_angles[i*6+5] = M_PI - angle(n0, n2);
    }
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <fstream>
#include <iostream>
using namespace std;
using namespace boost;

typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_index_t, int>, property<edge_index_t, int>> Graph;
typedef vector<vector<graph_traits<Graph>::edge_descriptor>>                                                embedding_storage_t;
typedef iterator_property_map<embedding_storage_t::iterator, property_map<Graph, vertex_index_t>::type>     embedding_t;

struct face_counter : planar_face_traversal_visitor
{
        face_counter() : count(0) {}
        void begin_face() { ++count; }
        uint count;
};

struct coord_t
{
        size_t x, y;
};

void make_max_planar(Graph& g)
{
        auto e_index = get(edge_index, g);
        graph_traits<Graph>::edges_size_type edge_count = 0;
        graph_traits<Graph>::edge_iterator ei, ei_end;
        for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) put(e_index, *ei, edge_count++);

        typedef vector<graph_traits<Graph>::edge_descriptor> vec_t;
        vector<vec_t> embedding(num_vertices(g));
        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = &embedding[0]);

        make_biconnected_planar(g, &embedding[0]);

        edge_count = 0;
        for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) put(e_index, *ei, edge_count++);

        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = &embedding[0]);

        make_maximal_planar(g, &embedding[0]);

        edge_count = 0;
        for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) put(e_index, *ei, edge_count++);

        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = &embedding[0]);

        face_counter count_visitor;
        planar_face_traversal(g, &embedding[0], count_visitor);
}

int main(int argc, char** argv)
{
        if( argc < 2 ){
                cerr << "Usage: straightline [filename]\n";
                return 1;
        }

        string fname(argv[1]);
        ifstream f(fname);
        if( !f ){
                cerr << "file " << fname << " not found!\n";
                return 1;
        }

        string str;
        vector<pair<uint, uint>> edges;
        uint n = 0;
        while( getline(f, str) ){
                uint   colon = str.find(",");
                string stra  = str.substr(0, colon); trim(stra);
                string strb  = str.substr(colon+1 ); trim(strb);
                uint   a     = lexical_cast<uint>(stra);
                uint   b     = lexical_cast<uint>(strb);
                n = max(max(n, a), b);
                edges.push_back(make_pair(a, b));
        }

        Graph g(n);
        for( auto& e : edges ) add_edge(e.first, e.second, g);
        make_max_planar(g);

        embedding_storage_t embedding_storage(num_vertices(g));
        embedding_t         embedding        (embedding_storage.begin(), get(vertex_index,g));

        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

        vector<graph_traits<Graph>::vertex_descriptor> ordering;
        planar_canonical_ordering(g, embedding, back_inserter(ordering));

        typedef vector<coord_t> 											      straight_line_drawing_storage_t;
        typedef iterator_property_map < straight_line_drawing_storage_t::iterator, property_map<Graph, vertex_index_t>::type> straight_line_drawing_t;

        straight_line_drawing_storage_t straight_line_drawing_storage (num_vertices(g));
        straight_line_drawing_t straight_line_drawing (straight_line_drawing_storage.begin(), get(vertex_index,g));

        chrobak_payne_straight_line_drawing(g, embedding, ordering.begin(), ordering.end(), straight_line_drawing);

        graph_traits<Graph>::vertex_iterator vi, vi_end;
        cout << "graph G {\n";
        for( tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi ){
                coord_t coord(get(straight_line_drawing,*vi));
                cout << *vi << "[pos=\"" << coord.x << ',' << coord.y << "!\"];\n";
        }
        for( auto& e : edges ) cout << e.first << "--" << e.second << " ;\n";
        cout << "}\n";
}


#include <boost/numeric/mtl/mtl.hpp>

int main()
try {
    using namespace mtl;

    dense2D<int> A(2, 2), B(2, 2), C(4, 4);

    for (size_t r= 0; r < 2; ++r)
        for (size_t c= 0; c < 2; ++c) {
            A[r][c]= (r+1) * 10 + c+1;
            B[r][c]= (r+1) * 1000 + (c+1) * 100;
        }

    C= kron(A, B);
    std::cout << "kron(A, B) is\n" << C;

    MTL_THROW_IF(C[0][0] != 12100, mtl::runtime_error("Wrong value in C[0][0]"));
    MTL_THROW_IF(C[3][3] != 48400, mtl::runtime_error("Wrong value in C[3][3]"));

    return EXIT_SUCCESS;
}
catch (const mtl::runtime_error& e) {
    std::cerr << "Caught an MTL runtime error: " << e.what() << std::endl;
    return EXIT_FAILURE;
}


//when you need a function, just find that function and include a header file
//also need to get eigen in your cmake file
//getting th, v values straight from IMU
#include <iostream>
#include <Eigen/Dense>
#include <math.h>

//Global Variables
using Eigen::Vector3f;
Vector3f g(0,0,-9.81); //gravity

//Cross Product Equivalent
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f crossProductEquivalent(Vector3f v)
{
  Matrix3f c;
  c << 0, -v(2), v(1),
       v(2), 0, -v(0),
       -v(1), v(0), 0;
  //std::cout << c << std::endl;
  return c;
}

//Quaternion Multiplication
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qMultiply(Quaternionf q1, Quaternionf q2)
{
  float w1 = q1.w();
  float w2 = q2.w();
  Vector3f v1 = q1.vec();
  Vector3f v2 = q2.vec();
  float wReturn = w1*w2 - v1(0)*v2(0) - v1(1)*v2(1) - v1(2)*v2(2);
  Vector3f vReturn;
  vReturn(0) = w1*v2(0) + v1(0)*w2 + v1(1)*v2(2) - v1(2)*v2(1);
  vReturn(1) = w1*v2(1) - v1(0)*v2(2) + v1(1)*w2 + v1(2)*v2(0);
  vReturn(2) = w1*v2(2) + v1(0)*v2(1) - v1(1)*v2(0) + v1(2)*w2;
  Quaternionf qReturn;
  qReturn.w() = wReturn;
  qReturn.vec() = vReturn;
  //std::cout << qReturn.w() << std::endl << qReturn.vec() << std::endl;
  return qReturn;
}

//Quaternion Exponential
//implements simple 0th-order integration
//ref: quaternion kinematics, section 4.6.1
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qExponential(float dt, Vector3f w)
{
  float wn = w.norm();
  Vector3f wN = w.normalized();
  Quaternionf qReturn;
  qReturn.w() = cos(wn * dt / 2);
  qReturn.vec() = wN * sin(wn * dt / 2);
  return qReturn;
}

int main()
{

//Propagation

//Execute each time the IMU is sampled
Vector3f am; //accelerometer measurement (get from IMU)
Vector3f wm; //gyroscope measurement (get from IMU)
float dt; //(get from ROS)

//Build Omega(w)
wmx = crossProductEquivalent(wm);
Matrix 4f Omega;
Omega << -wmx(0,0), -wmx(0,1), -wmx(0,2), wm(0),
	 -wmx(1,0), -wmx(1,1), -wmx(1,2), wm(1),
         -wmx(2,0), -wmx(2,1), -wmx(2,2), wm(2),
         -wm(0), -wm(1), -wm(2), 0;

//Measurements at time l-1
Vector3f amOld; //accelerometer measurement from last time
Vector3f wmOld; //gyroscope measurement from last time

//Propagate state estimate

// Constants
using Eigen::Quaternionf;
using Eigen::Vector3f;
using Eigen::Matrix3f
Matrix3f I3 = Matrix3f::Identity(3,3);
Matrix3f O3 = Matrix3f::Zero();
float g = 9.81;

//Propagate quaternion
using Eigen::Quaternionf;
using Eigen::Vector3f;
using Eigen::Matrix3f
Quaternionf qHat;
Matrix3f RHat = qHat.toRotationMatrix();
Matrix3f RHatProp = (I - dt * wmx) * RHat;
Quaternionf qHatExp = qExponential(wmOld, dt);
Quaternionf qHatProp = qMultiply(qHatExp, qHatExp);

//Propagate p, the position
Vector3f pHatProp = pHat + vHat*dt + RHat*RHatProp*(amOld - baHat)*dt^2 + 0.5*g*(dt^2)

//Propagate v, the velocity
Vector3f vHatProp = vHat + RHat*RHatProp*(amOld - baHat)*dt + g*dt;

//Propagate bg and ba, the gyroscope and accelerometer biases
Vector3f bgHatProp = bgHat;
Vector3f baHatProp = baHat;

//Calculate IMU error state transition matrix
using Eigen::MatrixXf;
phipq = -crossProductEquivalent(pHatProp - pHat - vHat*dt - 0.5*g*dt^2);
phivq = -crossProductEquivalent(vHatProp - vHat - g*dt);
phigbg = RHat.transpose() * RHatProp * dt;
phipbg = crossProductEquivalent(vHat - g*dt) * RHat.transpose() * RHatProp * dt;
phipa = RHat.transpose() * RHatProp * dt^2;
phivbg = crossProductEquivalent(vHat - g*dt) * RHat.transpose() * RHatProp * dt;
phiva = RHat.transpose() * RHatProp * dt;
MatrixXf PhiProp(15, 15);
PhiProp << I3,    O3,      O3, phiqbg,    O3,
	         phipq, I3, (dt*I3), phipbg, phipa,
           phivq, O3,      I3, phivbg, phiva,
	         O3,    O3,      O3,     I3,    O3,
           O3,    O3,      O3,     O3,    I3;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <typeinfo>

template <typename At, typename Ut>
void dense_ic_0(const At& As, const Ut& Us)
{
    mtl::dense2D<double> U(upper(As));

    const std::size_t n= num_rows(U);

    for (std::size_t k= 0; k < n; k++) {
	double dia= U[k][k]= sqrt(U[k][k]);
	for (std::size_t i = k + 1; i < n; i++) {
	    double d= U[k][i] /= dia;
	    for (std::size_t j = k + 1; j <= i; j++)
		if (U[j][i] != 0.0)
		    U[j][i] -= d * U[k][j];
	}
    }

    std::cout << "Factorizing A = \n" << As << "-> U = \n" << with_format(U, 6, 2)
	      << "trans(U) * U = \n" << with_format(mtl::dense2D<double>(trans(U) * U), 6, 2);

    if (std::abs(U[2][3] - Us[2][3]) > 0.001) throw "Wrong value in L for sparse IC(0) factorization";

    if (std::abs(U[3][3] - 1. / Us[3][3]) > 0.001) throw "Wrong value in U for sparse IC(0) factorization";
}

template <typename Solver>
void test(const Solver&)
{
    typedef typename mtl::ashape::ashape<Solver>::type shape;
    std::cout << "type is " << typeid(Solver).name() << '\n';
    std::cout << "ashape is " << typeid(shape).name() << '\n';
    std::cout << "type is " << (mtl::traits::is_scalar<Solver>::value ? "" : "not ") << "scalar\n";
    std::cout << "type is " << (mtl::traits::backward_index_evaluatable<Solver>::value ? "" : "not ") << "back-eval\n";
}

int main()
{
    // For a more realistic example set sz to 1000 or larger
    const int size = 3, N = size * size;

    typedef mtl::compressed2D<double>  matrix_type;
    mtl::compressed2D<double>          A(N, N), dia(N, N);
    laplacian_setup(A, size, size);
    // dia= 1.0; A+= dia;

    itl::pc::ic_0<matrix_type, float>  P(A);
    mtl::dense_vector<double>          x(N, 1.0), b(N);

    if(size > 1 && size < 4)
	dense_ic_0(A, P.get_U());

    b = A * x;
    x= 0;

    itl::cyclic_iteration<double> iter(b, N, 1.e-6, 0.0, 1);
    cg(A, x, b, P, iter);

    // test(mtl::lazy(b)= solve(P, x));

    return 0;
}


#include <Eigen/Dense>
#include "Geometry.hh"
#include "PeriodicTable.hh"
#include "XYZMatrix.hh"
#include "ZMatrix.hh"
#include "io/manipulators.hh"
#include "exceptions.hh"

double Geometry::nuclearRepulsion() const
{
	double sum = 0;

	for (int j = 1; j < size(); ++j)
	{
		sum += (charges().head(j).transpose().array()
			/ (positions().block(0, 0, 3, j).colwise() - position(j))
				.colwise().norm().array())
			.sum();
	}

	return sum;
}

void Geometry::setAtom(int idx, const std::string& symbol,
	double x, double y, double z)
{
	checkIndex(idx);

	const Element& elem = PeriodicTable::singleton().findBySymbol(symbol);

	_positions.col(idx) << x, y, z;
	_masses(idx) = elem.mass();
	_charges(idx) = elem.number();
	_symbols[idx] = symbol;
}

std::ostream& Geometry::print(std::ostream& os) const
{
	os << "Geometry (\n" << indent;
	for (int i = 0; i < size(); i++)
		os << _charges(i) << "\t" << _masses(i) << "\t"
			<< symbol(i) << "\t"
			<< position(i).transpose() << "\n";
	os << dedent << ")";
	return os;
}

JobIStream& Geometry::scan(JobIStream& is)
{
	is >> getline;
	if (is.eof())
		throw UnexpectedEOF();

	std::string elem;
	double x, y, z;
	is >> element(elem, false) >> x >> y >> z;
	if (!is.fail())
	{
		is.ungetLastLine();

		XYZMatrix mat;
		is >> mat;
		mat.fillGeometry(this);
	}
	else
	{
		is.ungetLastLine();

		ZMatrix mat;
		is >> mat;
		mat.fillGeometry(this);
	}

	return is;
}

void Geometry::toPrincipalAxes()
{
	// Move center of mass to origin
	Eigen::Vector3d cm = (positions() * masses()) / masses().sum();
	_positions.colwise() -= cm;

	// Compute principal axes
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(inertia());
	const Eigen::Matrix3d& axes = solver.eigenvectors();
	Eigen::Vector3d c = axes.col(0), b = axes.col(1), a = axes.col(0);
	if (a.cross(b).dot(c) < 0)
		// axis system is left-handed, flip one
		a = -a;

	// First two angles put the z axis in place
	double theta = std::acos(c.z());
	double psi = theta > 2 * std::numeric_limits<double>::epsilon()
		? std::atan2(c.y(), c.x()) : 0;

	// Rotate the a axis into the xy-plane
	Eigen::Matrix3d R = (Eigen::AngleAxisd(-theta, Eigen::Vector3d::UnitY())
		* Eigen::AngleAxisd(-psi, Eigen::Vector3d::UnitZ()))
		.toRotationMatrix();
	a = R * a;

	// Last angle aligns a axis with x (and b with y)
	double phi = std::atan2(a.y(), a.x());
	R = Eigen::AngleAxisd(-phi, Eigen::Vector3d::UnitZ()) * R;
	_positions = R * positions();
}

Eigen::Matrix3d Geometry::inertia() const
{
	Eigen::Matrix3d I;
        for (int j = 0; j < 3; j++)
        {
                for (int i = 0; i <= j; i++)
                {
			I(j,i) = I(i,j) =
				-_positions.row(i).cwiseProduct(_positions.row(j))
					.dot(masses());
		}
        }

        double trace = I.trace();
        for (int i = 0; i < 3; i++)
		I(i,i) += trace;

	return I;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Written by Cornelius Steinhardt

#include <cmath>
#include <string>

// #include <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename Matrix>
void test1(Matrix& m, double tau)
{
  mtl::mat::inserter<Matrix> ins(m);
  size_t nrows=num_rows(m);
  double val;
  for (size_t r=0;r<nrows;++r)
  {
    for (size_t c=0;c<nrows;++c)
    {
      if(r==c)
        ins(r,c) << 1.;
      else
      {
        val=2.*(static_cast<double>(rand())/RAND_MAX - 0.5);
        if (val<tau)
          ins(r,c) << val;
      }
    }
  }
}

template <typename Matrix, typename Vector, typename Left, typename Right>
void test(char const* name, char const* comment, Matrix const& A, Vector& x, Vector const& b, Left const& L, Right const& R,
	  unsigned restart, bool check_convergence= true)
{
    const int Niter = 100;

    std::cout << name << comment << "\n";
    x= 2.0, 3., 4., 8;

    itl::cyclic_iteration<double> iter(b, Niter, 1.e-8, 0.0, 10);
    gmres(A, x, b, L, R, iter, restart);
    std::cout << "x= " << x << " \n" ;
    Vector r(b - A*x);
    if (false && check_convergence && two_norm(r) > 0.00001)
	throw std::string(name) + std::string(" doesn't converge!");
}

int main(int, char**)
{
    const int N = 2;
    typedef mtl::compressed2D<double> matrix_type;
    matrix_type                   A(N*N, N*N);
    laplacian_setup(A, N, N);

    mtl::dense_vector<double> b(N*N, 1), x(N*N,1), r(N*N);

    itl::pc::identity<matrix_type>         Ident(A);
    itl::pc::ic_0<matrix_type>             ic(A);
    itl::pc::ilu_0<matrix_type>            ilu(A);
    itl::pc::diagonal<matrix_type>         diag(A);

    std::cout << "A has " << A.nnz() << " non-zero entries" << std::endl;
    std::cout << "A =\n" << A << " \n";

    test("Non-preconditioned GMRES(1)", "\nWon't convergence (for large examples,without restarts)!",
	 A, x, b, Ident, Ident, 1, false);
    test("Non-preconditioned GMRES(4)", "", A, x, b, Ident, Ident, 4);
    test("Left ILU(0) GMRES(4)", "", A, x, b, ilu, Ident, 4);
    test("Left IC(0) GMRES(4)", "", A, x, b, ic, Ident, 4);
    test("Left diag GMRES(4)", "", A, x, b, diag, Ident, 4);

    test("Right ILU(0) GMRES(4)", "", A, x, b, Ident, ilu, 4);
    test("Right IC(0) GMRES(4)", "", A, x, b, Ident, ic, 4);
    test("Right diag GMRES(4)", "", A, x, b, Ident, diag, 4);

    test("Left ILU(0) Right ILU(0) GMRES(4)", "", A, x, b, ilu, ilu, 4);
    test("Left ILU(0) Right IC(0) GMRES(4)", "", A, x, b, ilu, ic, 4);
    test("Left ILU(0) Right diag GMRES(4)", "", A, x, b, ilu, diag, 4);

    return 0;
}


// inverting symmetric/hermitian positive definite
// factor (potrf()) and invert (potri())

// #define BOOST_UBLAS_STRICT_HERMITIAN
// .. doesn't work (yet?)

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/lapack/computational/potri.hpp>
#include <boost/numeric/bindings/lapack/computational/potrf.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef double real_t;

typedef std::complex<real_t> cmplx_t;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
#else
typedef ublas::matrix<real_t, ublas::row_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> cm_t;
#endif

#ifndef F_UPPER
typedef ublas::symmetric_adaptor<m_t, ublas::lower> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> herm_t;
#else
typedef ublas::symmetric_adaptor<m_t, ublas::upper> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::upper> herm_t;
#endif

int main() {

  cout << endl;

  cout << "real symmetric\n" << endl;

  size_t n = 3;
  m_t a (n, n);    // matrix (storage)
  symm_t sa (a);   // symmetric adaptor

#ifdef F_UPPER
  init_symm (sa, 'u');
#else
  init_symm (sa, 'l');
#endif
  // ifdef F_UPPER
  //        [5 4 3 2 1]
  //        [0 5 4 3 2]
  //    a = [0 0 5 4 3]
  //        [0 0 0 5 4]
  //        [0 0 0 0 n]
  // else
  //        [5 0 0 0 0]
  //        [4 5 0 0 0]
  //    a = [3 4 5 0 0]
  //        [2 3 4 5 0]
  //        [1 2 3 4 5]
  print_m (sa, "A");
  cout << endl;

  m_t a2 (sa);   // full symmetric copy of sa:
                 // .. sa is `lost' after potrf();
                 // .. only one parameter of symm() is symmetric matrix

  int ierr = lapack::potrf (sa);
  if (!ierr) {
    lapack::potri (sa);
    // ri should be (almost) identity matrix:
    m_t ri (n, n);
    blas::symm ( bindings::tag::right(), 1.0, sa, a2, 0.0, ri);
    print_m (ri, "I = A * A^(-1)");
    cout << endl;
    blas::symm ( bindings::tag::left(), 1.0, sa, a2, 0.0, ri);
    print_m (ri, "I = A^(-1) * A");
    cout << endl;
  }

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian (almost ;o)\n" << endl;

  // hermitian
  cm_t ca (3, 3);
  herm_t ha (ca);

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (3, 0);
  ha (1, 0) = cmplx_t (2, 0);
  ha (1, 1) = cmplx_t (3, 0);
  ha (2, 0) = cmplx_t (1, 0);
  ha (2, 1) = cmplx_t (2, 0);
  ha (2, 2) = cmplx_t (3, 0);
#else
  ha (0, 0) = cmplx_t (3, 0);
  ha (0, 1) = cmplx_t (2, 0);
  ha (0, 2) = cmplx_t (1, 0);
  ha (1, 1) = cmplx_t (3, 0);
  ha (1, 2) = cmplx_t (2, 0);
  ha (2, 2) = cmplx_t (3, 0);
#endif

  print_m (ha, "A");
  cout << endl;

  cm_t ca2 (ha);  // full hermitian

  ierr = lapack::potri (ha);   // potrf()
  if (ierr == 0) {
    lapack::potri (ha);        // potri()
    cm_t ic (3, 3);
    blas::hemm ( bindings::tag::right(), 1.0, ha, ca2, 0.0, ic);
    print_m (ic, "I = A * A^(-1)");
    cout << endl;
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (25, 0);
  ha (1, 0) = cmplx_t (-5, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (2, 0) = cmplx_t (10, -5);
  ha (2, 1) = cmplx_t (4, 6);
  ha (2, 2) = cmplx_t (71, 0);
#else
  ha (0, 0) = cmplx_t (25, 0);
  ha (0, 1) = cmplx_t (-5, -5);
  ha (0, 2) = cmplx_t (10, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (1, 2) = cmplx_t (4, -6);
  ha (2, 2) = cmplx_t (71, 0);
#endif
  print_m (ha, "A");
  cout << endl;

  ca2 = ha;

  ierr = lapack::potrf (ha);
  if (ierr == 0) {
    lapack::potri (ha);
    cm_t ic (3, 3);
    blas::hemm ( bindings::tag::right(), 1.0, ha, ca2, 0.0, ic);
    print_m (ic, "I = A * A^(-1)");
    cout << endl;
    blas::hemm ( bindings::tag::left(), 1.0, ha, ca2, 0.0, ic);
    print_m (ic, "I = A^(-1) * A");
    cout << endl;
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;

  cout << endl;

}


#pragma once
#include "math_util.hpp"
#include <boost/assert.hpp>
#include <boost/operators.hpp>
#include <cmath>
#include <limits>
#include <numeric>

namespace dmc
{
	template <class Scalar, int Dimension>
	class vector;

	template <class Derived, class Scalar, int Dimension>
	class vector_base
		: boost::addable<
			  Derived,
			  boost::subtractable<
				  Derived,
				  boost::multipliable<
					  Derived,
					  Scalar,
					  boost::dividable<
						  Derived,
						  Scalar,
						  boost::equality_comparable<
							  Derived>>>>>
	{
	public:
		typedef Scalar scalar_type;
		static const int dimension = Dimension;

		scalar_type* data()
		{
			return &values_[0];
		}

		const scalar_type* data() const
		{
			return &values_[0];
		}

		scalar_type& operator[](int index)
		{
			BOOST_ASSERT(0 <= index && index < dimension);
			return values_[index];
		}

		scalar_type operator[](int index) const
		{
			BOOST_ASSERT(0 <= index && index < dimension);
			return values_[index];
		}

		Derived operator-() const
		{
			return map([](auto x) { return -x; });
		}

		const Derived& operator+() const
		{
			return derived();
		}

		Derived& operator+=(const Derived& rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] += rhs[i];
			return derived();
		}

		Derived& operator-=(const Derived& rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] -= rhs[i];
			return derived();
		}

		Derived& operator*=(scalar_type rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] *= rhs;
			return derived();
		}

		Derived& operator/=(scalar_type rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] /= rhs;
			return derived();
		}

		template <class F>
		vector<typename std::result_of<F(scalar_type)>::type, dimension> map(F f) const
		{
			vector<typename std::result_of<F(scalar_type)>::type, dimension> result;
			for (int i = 0; i < dimension; ++i)
				result[i] = f((*this)[i]);
			return result;
		}

		template <class T>
		auto cast() const
		{
			return map([](auto x) { return static_cast<T>(x); });
		}

		auto sign() const
		{
			return map([](auto x) { return dmc::sign(x); });
		}

		template <class T, class F>
		auto reduce(T t, F f) const
		{
			for (int i = 0; i < dimension; ++i)
				t = f(t, (*this)[i]);
			return t;
		}

		template <class F>
		auto reduce(F f) const
		{
			auto t = (*this)[0];
			for (int i = 1; i < dimension; ++i)
				t = f(t, (*this)[i]);
			return t;
		}

		auto sum() const
		{
			return reduce([](auto x, auto y) {
				return x + y;
			});
		}

		auto product() const
		{
			return reduce([](auto x, auto y) {
				return x * y;
			});
		}

		auto abs() const
		{
			return map([](auto x) { using std::abs; return abs(x); });
		}

		auto norm_l1() const
		{
			return abs().sum();
		}

		auto squared() const
		{
			return map([](auto x) { return squared(x); });
		}

		auto norm_l2_sq() const
		{
			return squared().sum();
		}

		auto norm_l2() const
		{
			using std::sqrt;
			return sqrt(norm_l2_sq());
		}

		auto max() const
		{
			return reduce([](auto x, auto y) { using std::max; return max(x, y); });
		}

		auto min() const
		{
			return reduce([](auto x, auto y) { using std::min; return min(x, y); });
		}

		bool try_normalize()
		{
			auto n = norm_l2();
			if (n < std::numeric_limits<scalar_type>::epsilon())
				return false;

			*this /= n;
			return true;
		}

		Derived clamp(const Derived& minimum, const Derived& maximum)
		{
			Derived result;
			for (int i = 0; i < dimension; ++i)
				result[i] = std::max(minimum[i], std::min(maximum[i], (*this)[i]));
			return result;
		}

		static Derived all(scalar_type s)
		{
			Derived result;
			for (int i = 0; i < dimension; ++i)
				result[i] = s;
			return result;
		}

		friend bool operator==(const Derived& lhs, const Derived& rhs)
		{
			for (int i = 0; i < dimension; ++i)
				if (lhs[i] != rhs[i])
					return false;

			return true;
		}

	private:
		Derived& derived()
		{
			return static_cast<Derived&>(*this);
		}

		const Derived& derived() const
		{
			return static_cast<const Derived&>(*this);
		}

		scalar_type values_[dimension] = {};
	};
}


#pragma once

#include <algorithm>
#include <boost/range.hpp>
#include <math.h>
#include <math/Vec.hpp>
#include <utility>

template <int rows_, int cols_, class T = float>
struct Matrix {
	static const int rows = rows_;
	static const int cols = cols_;
	static const int size = rows * cols;

	typedef T value_type;

	typedef T & reference;
	typedef T const & const_reference;

	typedef T * pointer;
	typedef T const * const_pointer;

	typedef T row_type[cols];
	typedef row_type storage_type[rows];

	typedef pointer iterator;
	typedef const_pointer const_iterator;

	typedef std::pair<iterator, iterator> range_type;
	typedef std::pair<const_iterator, const_iterator> const_range_type;

	Matrix() {
		iterator end = this->end();
		for (iterator it = this->begin(); it != end; *it++ = 0) { ; }
	}

	template <class InIt>
	Matrix(InIt it, InIt end) {
		assign(it, end);
	}

	template <class U>
	Matrix(U (&array)[rows * cols]) {
		assign(boost::begin(array), boost::end(array));
	}

	template <class R>
	Matrix(R (&array)[rows]) {
		iterator out = begin();
		for (int i = 0; i < rows; ++i) {
			out = std::copy(boost::begin(array[i]), boost::end(array[i]), out);
		}
	}

	row_type & operator [] (int i) { return data_[i]; }
	row_type const & operator [] (int i) const { return data_[i]; }

	reference operator () (int i, int j) { return data_[i][j]; }
	const_reference operator () (int i, int j) const { return data_[i][j]; }

	range_type row_range(int i) { return range_type(data_[i], data_[i] + cols); }
	const_range_type row_range(int i) const { return const_range_type(data_[i], data_[i] + cols); }

	range_type column_range(int j) { return range_type(data_[0] + j, data_[rows] + j); }
	const_range_type column_range(int j) const { return const_range_type(data_[0] + j, data_[rows] + j); }

	iterator begin() { return data_[0]; }
	const_iterator begin() const { return data_[0]; }

	iterator end() { return data_[rows]; }
	const_iterator end() const { return data_[rows]; }

	template <class It>
	void assign(It it, It end) {
		iterator out_end = this->end();
		for (iterator out = begin(); out != out_end && it != end; *out++ = *it++) { ; }
	}

	template <class U>
	Vec<rows, T> operator * (Vec<cols, U> const & v) const {
		Vec<rows, T> out;
		for (int i = 0; i < rows; ++i) {
			out[i] = dot(row_range(i), 1, std::make_pair(v.begin(), v.end()), 1);
		}

		return out;
	}

	template <class U>
	friend Vec<cols, T> operator * (Vec<rows, U> const & v, Matrix const & M) {
		Vec<cols, T> out;
		for (int i = 0; i < cols; ++i) {
			out[i] = M.dot(std::make_pair(v.begin(), v.end()), 1, M.column_range(i), cols);
		}

		return out;
	}

	template <int c, class U>
	Matrix<rows, c, T> operator * (Matrix<cols, c, U> const & right) const {
		Matrix<rows, c> out;
		for (int i = 0; i < rows; ++i) {
			for (int j = 0; j < right.cols; ++j) {
				out.data_[i][j] = dot(row_range(i), 1, right.column_range(j), right.cols);
			}
		}
		return out;
	}

	template <class U>
	Matrix & operator *= (Matrix<cols, cols, U> const & right) {
		*this = (*this) * right;
		return *this;
	}

	template <class stream>
	friend stream & operator << (stream & out, Matrix const & m) {
		out << "{ ";
		for (int i = 0; i < m.rows; ++i) {
			if (i) { out << ", "; }

			out << "{ ";
			for (int j = 0; j < m.cols; ++j) {
				if (j) { out << ", "; }
				out << m.data_[i][j];
			}
			out << " }";
		}
		out << " }";
		return out;
	}

	void set_row(int i, Vec<cols, T> const & row) {
		range_type r = row_range(i);
		for (int j = 0; j < cols; ++j) { r.first[j] = row[j]; }
	}

	void set_column(int j, Vec<rows, T> const & col) {
		range_type r = column_range(j);
		for (int i = 0; i < rows; ++i) { r.first[i] = col[i]; }
	}

	private:
		template <class L, class R>
		static inline T dot(L left, int stride_left, R right, int stride_right) {
			T out = 0;

			while (left.first < left.second && right.first < right.second) {
				out += *left.first * *right.first;
				left.first += stride_left;
				right.first += stride_right;
			}

			return out;
		}

		storage_type data_;
};

namespace detail {
	namespace MatrixInverter {
		inline int abs(int x) { return ::abs(x); }
		inline float abs(float x) { return fabsf(x); }
		inline double abs(double x) { return fabs(x); }
		inline long double abs(long double x) { return fabsl(x); }

		template <class T, int N>
		inline void swap(T (&left)[N], T (&right)[N]) {
			float *l = left, *r = right;
			for (int i = 0; i < N; ++i) { std::swap(*l++, *r++); }
		}

		template <class T>
		inline void div(T * begin, T * end, T d) {
			for (; begin != end; ++begin) { *begin /= d; }
		}

		template <class T>
		inline T max_abs(T * begin, T * end) {
			T m = 0;
			for (T * it = begin; it < end; ++it) {
				T v = abs(*it);
				if (it != begin && v <= m) { continue; }
				m = v;
			}

			return m;
		}

		template <class T>
		inline int find_pivot_row(T * begin, T * end, int stride) {
			T m = 0; int p = 0, i = 0;
			for (T * it = begin; it < end; ++i, it += stride) {
				T v = abs(*it);
				if (i && v <= m) { continue; }
				m = v; p = i;
			}

			return p;
		}

		template <class T>
		inline void normalize_front(T * begin, T * end) {
			float d = *begin; *begin = 1; ++begin;
			while (begin < end) { *begin++ /= d; }
		}

		template <class T>
		inline void scale_and_subtract(T * begin, T * end, T * out) {
			T s = -*out / *begin++; *out++ = 0;
			while (begin < end) { *out++ += s * *begin++; }
		}

		template <class T, int N>
		void invert_matrix(Matrix<N, N, T> * M) {
			T S[N][2 * N];

			for (int i = 0; i < N; ++i) {
				for (int j = 0; j < N; ++j) {
					S[i][j] = (*M)[i][j];
					S[i][j + N] = (i == j);
				}
			}

			for (int i = 0; i < N; ++i) { div(S[i], S[i] + 2 * N, max_abs(S[i], S[i] + N)); }

			for (int i = 0; i < N; ++i) {
				int pivot = find_pivot_row(S[i] + i, S[N] + i, 2 * N);
				if (pivot) { swap(S[i], S[i + pivot]); }

				normalize_front(S[i] + i, S[i] + 2 * N);
				for (int j = i + 1; j < N; ++j) { scale_and_subtract(S[i] + i, S[i] + 2 * N, S[j] + i); }
			}

			for (int i = N - 1; i > 0; --i) {
				for (int j = i - 1; j >= 0; --j) { scale_and_subtract(S[i] + i, S[i] + 2 * N, S[j] + i); }
			}

			for (int i = 0; i < N; ++i) {
				for (int j = 0; j < N; ++j) { (*M)[i][j] = S[i][j + N]; }
			}
		}
	}
}

using detail::MatrixInverter::invert_matrix;


#include <iostream>
#include <Eigen/Dense>

#include "Eigen/Eigen"  // AFTER GRIDMAP!
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

class SplineFunction {

  public:

      // The spline is used to interpolate antenna gain values, as we only have the graphs
      SplineFunction()
      {}

      SplineFunction(Eigen::VectorXd const &x_vec, Eigen::VectorXd const &y_vec)
        : x_min(x_vec.minCoeff()),
          x_max(x_vec.maxCoeff()),
          y_min(y_vec.minCoeff()),
          y_max(y_vec.maxCoeff()),
          // Spline fitting here. X values are scaled down to [0, 1] for this.
          spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(y_vec.transpose(), std::min<int>(x_vec.rows() - 1, 6), scaled_values(x_vec)))  // No more than cubic spline, but accept short vectors.
      {}

      // x values need to be scaled down in extraction as well.
      double interpDeg(double x) const {
        double y;
        y = spline_(scaled_value(x))(0);

        // interpolation may produce values bigger and lower than our limits ...
        y = std::max(std::min(y, y_max), y_min );
        return y;
      }

      double interpRad(double x) const {
          return interpDeg(x*180.0/M_PI);
      }

      // Helpers to scale X values down to [0, 1]
      double scaled_value(double x) const {
        return (x - x_min) / (x_max - x_min);
      }

    private:
      Eigen::RowVectorXd scaled_values(Eigen::VectorXd const &x_vec) const {
        return x_vec.unaryExpr([this](double x) { return scaled_value(x); }).transpose();
      }

      double x_min;
      double x_max;
      double y_min;
      double y_max;

      // Spline of one-dimensional "points."
      Eigen::Spline<double, 1> spline_;
};

/////////////////////////////

// quick build:
// g++ -I /usr/include/eigen3/ play_with_eigen.cpp -o play_with_eigen -std=c++11

using namespace std::placeholders;
using Eigen::MatrixXd;

const double C = 299792458.0;
const double SENSITIVITY = -115; // dB

const double TAG_LOSSES = -4.8;

const double LOSS_CONSTANT = 147.55;
const double freq= 865e6;
const double lambda =  C/freq;
const double ANTENNA_LOSSES_LIST [25] = {  -22.6, -25.2, -25, -20.2, -17.6, -15.6, -14, -11.2, -7.8, -5.2, -2.4, -0.6, 0, -0.6, -2.4, -5.2, -8.8, -12.2, -16.4, -19.2, -20.8, -24.4, -28.2, -24, -22.6};
const double ANTENNA_ANGLES_LIST [25] = {-180.0, -165.0, -150.0, -135.0, -120.0, -105.0, -90.0, -75.0, -60.0, -45.0, -30.0, -15.0, 0.0, 15.0, 30.0, 45.0, 60.0, 75.0, 90.0, 105.0, 120.0, 135.0, 150.0, 165.0, 180.0};

//! Generates a mesh, just like Matlab's meshgrid
//  Template specialization for column vectors (Eigen::VectorXd)
//  in : x, y column vectors
//       X, Y matrices, used to save the mesh
template <typename Scalar>
void meshgrid(const Eigen::Matrix<Scalar, -1, 1>& x,
              const Eigen::Matrix<Scalar, -1, 1>& y,
              Eigen::Matrix<Scalar, -1, -1>& X,
              Eigen::Matrix<Scalar, -1, -1>& Y) {
  const long nx = x.size(), ny = y.size();
  X.resize(ny, nx);
  Y.resize(ny, nx);
  for (long i = 0; i < ny; ++i) {
    X.row(i) = x.transpose();
  }

  // for (long j = 0; j < nx; ++j) {
  //   Y.col(j) = y;
  // }
  for (long j = 0; j < nx; ++j) {
    Y.col(j) = y.reverse();
  }
}

//! Generates a mesh, just like Matlab's meshgrid
//  Template specialization for row vectors (Eigen::RowVectorXd)
//  in : x, y row vectors
//       X, Y matrices, used to save the mesh
template <typename Scalar>
void meshgrid(const Eigen::Matrix<Scalar, 1, -1>& x,
              const Eigen::Matrix<Scalar, 1, -1>& y,
              Eigen::Matrix<Scalar, -1, -1>& X,
              Eigen::Matrix<Scalar, -1, -1>& Y) {
  Eigen::Matrix<Scalar, -1, 1> xt = x.transpose(),
                               yt = y.transpose();
  meshgrid(xt, yt, X, Y);
}

typedef Eigen::VectorXd Vec;
typedef Eigen::MatrixXd Mat;

int main(int argc, char **argv)
{
    int Nx, Ny, x_min, x_max, y_min, y_max;
    double x_m, y_m;
    Mat X, Y, R, A, propL, antL,totalLoss, rxPower;
    Vec x,y;

    double txPower = 0; //dB

    /////////////////////////        // build spline to interpolate antenna gains;
    std::vector<double> xVec(ANTENNA_ANGLES_LIST, ANTENNA_ANGLES_LIST + 25);
    Eigen::VectorXd xvals = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(xVec.data(), xVec.size());
    std::vector<double> yVec(ANTENNA_LOSSES_LIST, ANTENNA_LOSSES_LIST + 25);
    Eigen::VectorXd yvals= Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(yVec.data(), yVec.size());
    SplineFunction _antenna_gains= SplineFunction(xvals, yvals);
    ////////////////////////

    Nx = 5;
    Ny = 8;
    x_min = -1;
    x_max = 1;
    y_min = -2;
    y_max = 2;
    x_m = 0.5;
    y_m = 1.2;

    x = Vec::LinSpaced(Nx, x_min, x_max);
    y = Vec::LinSpaced(Ny, y_min, y_max);

    std::cout << "x" << std::endl;
    std::cout << x << std::endl;
    std::cout << "y" << std::endl;
    std::cout << y << std::endl;

    // create X,Y meshgrids
    meshgrid(x, y, X, Y);

    // distance to point m
    X = X.array() - x_m;
    Y = Y.array() - y_m;

    std::cout << "X" << std::endl;
    std::cout << X << std::endl;
    std::cout << "Y" << std::endl;
    std::cout << Y << std::endl;

    // create R,Ang matrixes
    R = (X.array().square() + Y.array().square()).array().sqrt();
    A = Y.binaryExpr(X, std::ptr_fun(atan2));

    std::cout << "R" << std::endl;
    std::cout << R << std::endl;

    std::cout << "A" << std::endl;
    std::cout << (A*180.0/3.141592) << std::endl;

    // Create a propagation matrix without taking obstacles
    auto funtor = std::bind(&SplineFunction::interpRad, _antenna_gains, _1) ;
    antL =  TAG_LOSSES + A.unaryExpr( funtor ).array();
    std::cout << "antL" << std::endl;
    std::cout << antL << std::endl;

    propL = LOSS_CONSTANT - (20.0 * (R * freq).unaryExpr(std::ptr_fun(log10))).array() ;
    std::cout << "propL" << std::endl;
    std::cout << propL << std::endl;

    // signal goes from antenna to tag and comes back again, so we double the losses
    totalLoss =  2*antL + 2*propL;
    std::cout << "totalLoss" << std::endl;
    std::cout << totalLoss << std::endl;

    rxPower = txPower + totalLoss.array();

    // this should remove points where friis is not applicable
    rxPower = (R.array()>2*lambda).select(rxPower,SENSITIVITY);
    // this should remove points where received power is too low
    rxPower = (rxPower.array()>SENSITIVITY).select(rxPower,SENSITIVITY);

    std::cout << "rxPower" << std::endl;
    std::cout << rxPower << std::endl;

}


#include <Eigen/Eigen>
#include <iostream>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
    int n = 10000;
  VectorXd x(n), b(n);
  SparseMatrix<double> A(n,n);
  /* ... fill A and b ... */
  BiCGSTAB<SparseMatrix<double> > solver;
  solver.compute(A);
  x = solver.solve(b);
  std::cout << "#iterations:     " << solver.iterations() << std::endl;
  std::cout << "estimated error: " << solver.error()      << std::endl;
  /* ... update b ... */
  x = solver.solve(b); // solve again
  return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 the Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120926    E. Dekens         File created.
 *
 *    References
 *
 *    Notes
 *
 */

#include <cmath>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"

namespace tudat
{
namespace basic_mathematics
{

//! Update maximum degree and order of cache
void SphericalHarmonicsCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder )
{
    maximumDegree_ = maximumDegree;
    maximumOrder_ = maximumOrder;

    legendreCache_->resetMaximumDegreeAndOrder( maximumDegree_, maximumOrder_ );

    sinesOfLongitude_.resize( maximumOrder_ + 1 );
    cosinesOfLongitude_.resize( maximumOrder_ + 1 );
    referenceRadiusRatioPowers_.resize( maximumDegree_ + 2 );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient(
        const double distance,
        const double radiusPowerTerm,
        const double cosineOfOrderLongitude,
        const double sineOfOrderLongitude,
        const double cosineOfLatitude,
        const double preMultiplier,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        const double legendrePolynomial,
        const double legendrePolynomialDerivative )
{
    // Return result.
    return ( Eigen::Vector3d( ) <<
             - preMultiplier / distance
             * radiusPowerTerm
             * ( static_cast< double >( degree ) + 1.0 ) * legendrePolynomial
             * ( cosineHarmonicCoefficient * cosineOfOrderLongitude
                 + sineHarmonicCoefficient * sineOfOrderLongitude ),
             preMultiplier * radiusPowerTerm
             * legendrePolynomialDerivative * cosineOfLatitude * (
                 cosineHarmonicCoefficient * cosineOfOrderLongitude
                 + sineHarmonicCoefficient * sineOfOrderLongitude ),
             preMultiplier * radiusPowerTerm
             * static_cast< double >( order ) * legendrePolynomial
             * ( sineHarmonicCoefficient * cosineOfOrderLongitude
                 - cosineHarmonicCoefficient * sineOfOrderLongitude ) ).finished( );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient(
        const Eigen::Vector3d& sphericalPosition,
        const double referenceRadius,
        const double preMultiplier,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        const double legendrePolynomial,
        const double legendrePolynomialDerivative )
{
    return computePotentialGradient(
                sphericalPosition( radiusIndex ),
                basic_mathematics::raiseToIntegerPower
                ( referenceRadius / sphericalPosition( radiusIndex ), static_cast< double >( degree ) + 1.0 ),
                std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ),
                std::sin( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ),
                std::cos( sphericalPosition( latitudeIndex ) ), preMultiplier, degree, order,
                cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition,
                                          const double preMultiplier,
                                          const int degree,
                                          const int order,
                                          const double cosineHarmonicCoefficient,
                                          const double sineHarmonicCoefficient,
                                          const double legendrePolynomial,
                                          const double legendrePolynomialDerivative,
                                          const boost::shared_ptr< SphericalHarmonicsCache > sphericalHarmonicsCache )
{
    return computePotentialGradient(
                sphericalPosition( radiusIndex ),
                sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ),
                sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ),
                sphericalHarmonicsCache->getSineOfMultipleLongitude( order ),
                sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ),
                preMultiplier, degree, order,
                cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative );
}

} // namespace basic_mathematics
} // namespace tudat