#include <iostream>
#include <fstream>
#include <algorithm>
#include <array>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Shape_detection/Efficient_RANSAC.h>
#include <CGAL/structure_point_set.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>
#include <CGAL/Advancing_front_surface_reconstruction.h>
#include <CGAL/IO/read_points.h>
#include <CGAL/disable_warnings.h>

#include <boost/lexical_cast.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel  Kernel;
typedef Kernel::Point_3  Point;
typedef std::pair<Kernel::Point_3, Kernel::Vector_3>         Point_with_normal;
typedef std::vector<Point_with_normal>                       Pwn_vector;
typedef CGAL::First_of_pair_property_map<Point_with_normal>  Point_map;
typedef CGAL::Second_of_pair_property_map<Point_with_normal> Normal_map;

// Efficient RANSAC types
typedef CGAL::Shape_detection::Efficient_RANSAC_traits
  <Kernel, Pwn_vector, Point_map, Normal_map>              Traits;
typedef CGAL::Shape_detection::Efficient_RANSAC<Traits>    Efficient_ransac;
typedef CGAL::Shape_detection::Plane<Traits>               Plane;

// Point set structuring type
typedef CGAL::Point_set_with_structure<Kernel>               Structure;

// Advancing front types
typedef CGAL::Advancing_front_surface_reconstruction_vertex_base_3<Kernel> LVb;
typedef CGAL::Advancing_front_surface_reconstruction_cell_base_3<Kernel> LCb;
typedef CGAL::Triangulation_data_structure_3<LVb,LCb> Tds;
typedef CGAL::Delaunay_triangulation_3<Kernel,Tds> Triangulation_3;
typedef Triangulation_3::Vertex_handle Vertex_handle;

typedef std::array<std::size_t,3> Facet;

// Functor to init the advancing front algorithm with indexed points
struct On_the_fly_pair{
  const Pwn_vector& points;
  typedef std::pair<Point, std::size_t> result_type;

  On_the_fly_pair(const Pwn_vector& points) : points(points) {}

  result_type
  operator()(std::size_t i) const
  {
    return result_type(points[i].first,i);
  }
};

// Specialized priority functor that favor structure coherence
template <typename Structure>
struct Priority_with_structure_coherence {

  Structure& structure;
  double bound;

  Priority_with_structure_coherence(Structure& structure,
                                    double bound)
    : structure (structure), bound (bound)
  {}

  template <typename AdvancingFront, typename Cell_handle>
  double operator() (AdvancingFront& adv, Cell_handle& c,
                     const int& index) const
  {
    // If perimeter > bound, return infinity so that facet is not used
    if (bound != 0)
      {
        double d  = 0;
        d = sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                                  c->vertex((index+2)%4)->point()));
        if(d>bound) return adv.infinity();
        d += sqrt(squared_distance(c->vertex((index+2)%4)->point(),
                                   c->vertex((index+3)%4)->point()));
        if(d>bound) return adv.infinity();
        d += sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                                   c->vertex((index+3)%4)->point()));
        if(d>bound) return adv.infinity();
      }

    Facet f = {{ c->vertex ((index + 1) % 4)->info (),
                 c->vertex ((index + 2) % 4)->info (),
                 c->vertex ((index + 3) % 4)->info () }};

    // facet_coherence takes values between -1 and 3, 3 being the most
    // coherent and -1 being incoherent. Smaller weight means higher
    // priority.
    double weight = 100. * (5 - structure.facet_coherence (f));

    return weight * adv.smallest_radius_delaunay_sphere (c, index);
  }

};

// Advancing front type
typedef CGAL::Advancing_front_surface_reconstruction
         <Triangulation_3,
          Priority_with_structure_coherence<Structure> >
        Reconstruction;

int main (int argc, char* argv[])
{
  // Points with normals.
  Pwn_vector points;

  const char* fname = (argc>1) ? argv[1] : "data/cube.pwn";
  // Loading point set from a file.

  if (!CGAL::read_points(fname, std::back_inserter(points),
                         CGAL::parameters::point_map(Point_map()).
                                           normal_map(Normal_map())))
  {
      std::cerr << "Error: cannot read file" << std::endl;
      return EXIT_FAILURE;
  }

  std::cerr << "Shape detection... ";

  Efficient_ransac ransac;
  ransac.set_input(points);
  ransac.add_shape_factory<Plane>(); // Only planes are useful for stucturing

  // Default RANSAC parameters
  Efficient_ransac::Parameters op;
  op.probability = 0.05;
  op.min_points = 100;
  op.epsilon = (argc>2 ? boost::lexical_cast<double>(argv[2]) : 0.002);
  op.cluster_epsilon = (argc>3 ? boost::lexical_cast<double>(argv[3]) : 0.02);
  op.normal_threshold = 0.7;

  ransac.detect(op); // Plane detection

  Efficient_ransac::Plane_range planes = ransac.planes();

  std::cerr << "done\nPoint set structuring... ";

  Pwn_vector structured_pts;
  Structure pss (points,
                 planes,
                 op.cluster_epsilon,  // Same parameter as RANSAC
                 CGAL::parameters::point_map (Point_map()).
                 normal_map (Normal_map()).
                 plane_map (CGAL::Shape_detection::Plane_map<Traits>()).
                 plane_index_map(CGAL::Shape_detection::Point_to_shape_index_map<Traits>(points, planes)));

  for (std::size_t i = 0; i < pss.size(); ++ i)
    structured_pts.push_back (pss[i]);

  std::cerr << "done\nAdvancing front... ";

  std::vector<std::size_t> point_indices(boost::counting_iterator<std::size_t>(0),
                                         boost::counting_iterator<std::size_t>(structured_pts.size()));

  Triangulation_3 dt (boost::make_transform_iterator(point_indices.begin(), On_the_fly_pair(structured_pts)),
                      boost::make_transform_iterator(point_indices.end(), On_the_fly_pair(structured_pts)));

  Priority_with_structure_coherence<Structure> priority (pss,
                                                         1000. * op.cluster_epsilon); // Avoid too large facets
  Reconstruction R(dt, priority);
  R.run ();

  std::cerr << "done\nWriting result... ";

  std::vector<Facet> output;
  const Reconstruction::TDS_2& tds = R.triangulation_data_structure_2();

  for(Reconstruction::TDS_2::Face_iterator fit = tds.faces_begin(); fit != tds.faces_end(); ++fit)
    if(fit->is_on_surface())
      output.push_back (CGAL::make_array(fit->vertex(0)->vertex_3()->id(),
                                         fit->vertex(1)->vertex_3()->id(),
                                         fit->vertex(2)->vertex_3()->id()));

  std::ofstream f ("out.off");
  f << "OFF\n" << structured_pts.size () << " " << output.size() << " 0\n"; // Header
  for (std::size_t i = 0; i < structured_pts.size (); ++ i)
    f << structured_pts[i].first << std::endl;
  for (std::size_t i = 0; i < output.size (); ++ i)
    f << "3 "
      << output[i][0] << " "
      << output[i][1] << " "
      << output[i][2] << std::endl;
  std::cerr << "all done\n" << std::endl;

  f.close();

  return 0;
}


/*
 * State.hpp
 *
 *  Created on: 30.10.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_MODELS_STATE_HPP_
#define ESTIMATORS_MODELS_STATE_HPP_
#include "../StateBoxes/StateBox.hpp"
#include <tuple>
#include "../../Plugins/sensor_plugin.hpp"
#include <boost/hana.hpp>
#include <boost/hana/ext/std/tuple.hpp>
#define INNER_TYPE 0
#define OUTER_TYPE 1

/**
 * The State Type Reader reads the sizes of the state boxes and tells the state how big the matrices have to be
 */
template<int inner, int outer, int input, typename ... ARGS>
class StateTypeReader {
public:
	static const int inner_, outer_;
};

template<int inner, int outer, int input, typename first, typename ... ARGS>
class StateTypeReader<inner, outer, input, first, ARGS...> : public StateTypeReader<inner + first::inner_size,
		outer + first::outer_size, input + first::input_size, ARGS ...> {

};

template<int inner, int outer, int input>
class StateTypeReader<inner, outer, input> {
public:
	static const int inner_size = inner;
	static const int outer_size = outer;
	static const int input_size = input;
	template<typename T>
	using OUTER_T=Eigen::Matrix<T,outer,1>;
	template<typename T>
	using INNER_T=Eigen::Matrix<T,inner,1>;
	template<typename T>
	using INPUT_T=Eigen::Matrix<T,input,1>;
};

namespace zavi
::estimator::model {
	/**
	 * default implementation for boxminus
	 * @param a
	 * @param b
	 * @return b boxminus a
	 */
	template< int measure_dim>
	struct base_measurement {
		static constexpr inline int output_size=measure_dim;
		template<typename T,typename T2>
		static auto boxminus(const Eigen::Matrix<T,measure_dim,1> & a, const Eigen::Matrix<T2,measure_dim,1> & b) {
			return b-a;
		}
	};

	/**
	 * The state is constructed from so called state boxes
	 * The state boxes implement a dynamic model, have inputs values, a boxplus and a boxminus operation
	 * This class just collects the state boxes from the STATE_BOXES tuple and passes eigen matrices to the functions of the boxes
	 * For Estimators, it provides a simple interface with  an eigen vector containing all states and functions to edit it
	 */
	template<typename ... STATE_BOXES>
	class State {
	public:
		// Reads the sizes and creates all types
		typedef StateTypeReader<0,0,0,STATE_BOXES ...> STATE_TYPE;

		typedef typename STATE_TYPE::template OUTER_T<double> OUTER_DOUBLE;
		static const int inner_size=STATE_TYPE::inner_size;
		static const int outer_size=STATE_TYPE::outer_size;
		static const int input_size=STATE_TYPE::input_size;
		static const int cost_size=inner_size-input_size;
		//static const int cost_size=outer_size-input_size;
		template<typename T>
		using OUTER_T=typename STATE_TYPE::template OUTER_T<T>;
		template<typename T>
		using INNER_T=typename STATE_TYPE::template INNER_T<T>;
		template<typename T>
		using INPUT_T=typename STATE_TYPE::template INPUT_T<T>;
		template<typename T>
		using COST_T=Eigen::Matrix<T,cost_size,1>;;
		typedef std::tuple<STATE_BOXES ...> BOXES_TUPLE;
		BOXES_TUPLE boxes;
		State(std::shared_ptr<plugin::SensorPlugin> sensor):boxes(initStates<0,STATE_BOXES ...>(sensor)) {

		}
		virtual ~State() {}
		/**
		 *
		 * @return The outer size of the state
		 */
		virtual int GlobalSize() const {
			return outer_size;
		}

		/**
		 *
		 * @return The inner size of the state
		 */
		virtual int LocalSize() const {
			return inner_size;
		}

		/**
		 * passes box_plus to ceres
		 * @param x
		 * @param delta
		 * @param x_plus_delta
		 * @return
		 */
		template<typename T>
		bool Plus(const T * x, const T* delta, T* x_plus_delta) const {
			(Eigen::Map<OUTER_T<T> >(x_plus_delta))=boxPlus<T>(Eigen::Map<const OUTER_T<T> >(x),Eigen::Map<const INNER_T<T> >(delta));
			return true;
		}

		/**
		 * Passes box_plus to ceres
		 * @param x
		 * @param delta
		 * @param x_plus_delta
		 * @return
		 */
		template<typename T>
		bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
			return Plus<T>(x,delta,x_plus_delta);

		}

		/**
		 * Pass the sensor to all state boxes
		 * @param sensor the sensor plugin (IMU PLugin)
		 * @return a tuple with all boxes
		 */
		template<int current_depth,typename T,typename ... rest>
		BOXES_TUPLE initStates(std::shared_ptr<plugin::SensorPlugin> sensor) {
			std::get<current_depth>(boxes)=T(sensor);
			return initStates<current_depth+1,rest ...>(sensor);
		}
		/**
		 * Just for the last recursion step, does nothing
		 * @param sensor
		 * @return
		 */
		template<int current_depth>
		BOXES_TUPLE initStates(std::shared_ptr<plugin::SensorPlugin> sensor) {
			return boxes;
		}

		/**
		 * Splits the state  and inner vector and passes them to all state boxes
		 * @param state
		 * @param delta
		 * @return state boxplis delta
		 */
		template<typename T>
		OUTER_T<T> boxPlus(const OUTER_T<T> & state,const INNER_T<T> & delta) const {
			OUTER_T<T> result=OUTER_T<T>::Zero();
			int outer_index=0,inner_index=0;
			boost::hana::for_each(boxes,[&result,&state,&delta,&outer_index, &inner_index](auto const & foo) {
						result.template block<foo.outer_size,1>(outer_index,0)=foo.template boxPlus<T>(state.template block<foo.outer_size,1>(outer_index,0),delta.template block<foo.inner_size,1>(inner_index,0));
						outer_index+=foo.outer_size;
						inner_index+=foo.inner_size;
					});
			return result;
		}
		/**
		 * as boxPlus, but for the inner state
		 * It is needed if the inner space itself is a manifold  (orientations)
		 * @param delta1
		 * @param delta2
		 * @return
		 */
		template<typename T>
		INNER_T<T> boxPlusInnerSpace(const INNER_T<T> & delta1,const INNER_T<T> & delta2) const {
			INNER_T<T> result=INNER_T<T>::Zero();
			int inner_index=0;
			boost::hana::for_each(boxes,[&result,&delta1,&delta2, &inner_index](auto const & foo) {
						result.template block<foo.inner_size,1>(inner_index,0)=foo.template boxPlusInnerSpace<T>(delta1.template block<foo.inner_size,1>(inner_index,0),delta2.template block<foo.inner_size,1>(inner_index,0));
						inner_index+=foo.inner_size;
					});
			return result;
		}
		/**
		 * As boxPlus but with boxMinus
		 * @param a
		 * @param b
		 * @return
		 */
		template<typename T>
		INNER_T<T> boxMinus(const OUTER_T<T> & a,const OUTER_T<T> & b) {
			INNER_T<T> result=INNER_T<T>::Zero();
			int outer_index=0,inner_index=0;
			boost::hana::for_each(boxes,[&result,&a,&b,&outer_index, &inner_index](auto & foo) {
						result.template block<foo.inner_size,1>(inner_index,0)=foo.template boxMinus<T>(a.template block<foo.outer_size,1>(outer_index,0),b.template block<foo.outer_size,1>(outer_index,0));
						outer_index+=foo.outer_size;
						inner_index+=foo.inner_size;
					});
			return result;
		}

		/**
		 * Calls the state transition function on all boxes
		 * @param state the current state
		 * @param time_diff the passed time till last call
		 * @return the new state
		 */
		template<typename T>
		OUTER_T<T> stateTransitionFunction(const OUTER_T<T> & state,const double time_diff) {
			OUTER_T<T> result=OUTER_T<T>::Zero();
			int outer_index=0;
			boost::hana::for_each(boxes,[&result,&state,time_diff,&outer_index](auto & foo) {
						result.template block<foo.outer_size,1>(outer_index,0)=foo.template stateTransition<T>(state.template block<foo.outer_size,1>(outer_index,0),time_diff);
						outer_index+=foo.outer_size;
					});
			return result;
		}
		/*template<typename T>
		 static INPUT_T<T> input_measurement(const OUTER_T<T> & state,void * prior) {
		 BOXES_TUPLE *box=static_cast<BOXES_TUPLE*>(*prior);
		 INPUT_T<T> result;
		 int outer_index=0,input_index=0;
		 boost::hana::for_each(*box,[&result,&state,&outer_index,&input_index](auto & foo) {
		 outer_index+=foo.outer_size;
		 result.template block<foo.input_size,1>(input_index,0)=state.template block<foo.input_size,1>(outer_index-foo.input_size,0);
		 input_index+=foo.input_size;
		 });
		 return result;
		 }*/

		/**
		 * Calculates the transition cost between to states scaled by stiffness
		 * @param state the current state
		 * @param time_diff the üassed time between 2 states
		 * @param after the next state
		 * @return stiffness*(after boxminus transition(state))
		 */
		template<typename T>
		COST_T<T> transitionCost(const OUTER_T<T> & state,const double time_diff, const OUTER_T<T> & after) {
			Eigen::Matrix<double,cost_size,cost_size> stiffnes=(getTransitionSTD(time_diff)+COST_T<double>::Ones()*1e-6).cwiseInverse().asDiagonal();
			//zavi::plot::liveHeatMap<0>(stiffnes," stiffnes of transition");
			COST_T<T> result=COST_T<T>::Zero();
			int outer_index=0,cost_index=0;
			boost::hana::for_each(boxes,[&result,&state,time_diff,&after,&outer_index,&cost_index](auto & foo) {
						result.template block<foo.inner_size-foo.input_size,1>(cost_index,0)=foo.template boxMinus<T>(foo.template stateTransition<T>(state.template block<foo.outer_size,1>(outer_index,0),time_diff),after.template block<foo.outer_size,1>(outer_index,0)).template block<foo.inner_size-foo.input_size,1>(0,0);
						//result.template block<foo.outer_size-foo.input_size,1>(cost_index,0)=(after.template block<foo.outer_size,1>(outer_index,0)-foo.template stateTransition<T>(state.template block<foo.outer_size,1>(outer_index,0),time_diff)).template block<foo.outer_size-foo.input_size,1>(0,0);
						cost_index+=foo.inner_size-foo.input_size;
						//cost_index+=foo.inner_size-foo.input_size;
						outer_index+=foo.outer_size;
					});
			assert_inputs(stiffnes,result);
			return stiffnes*result;
			//return result;
		}
		/**
		 * Get the transition standard deviation (without inputs)
		 * @param time_diff of the transition
		 * @return a cost_T vector containing the stds
		 */
		COST_T<double> getTransitionSTD(const double time_diff) {
			COST_T<double> result=COST_T<double>::Zero();
			int outer_index=0,cost_index=0;
			boost::hana::for_each(boxes,[&result,time_diff,&outer_index,&cost_index](auto & foo) {
						result.template block<foo.inner_size-foo.input_size,1>(cost_index,0)=foo.getSTD(time_diff).template block<foo.inner_size-foo.input_size,1>(0,0);
						cost_index+=foo.inner_size-foo.input_size;
						outer_index+=foo.outer_size;
					});
			return result;
		}
		/**
		 * Get the standard deviation  (with inputs)
		 * @param time_diff of the transition
		 * @return a cost_T vector containing the stds
		 */
		INNER_T<double> getStateSTD(const double time_diff) {
			INNER_T<double> result=INNER_T<double>::Zero();
			int inner_index=0;
			boost::hana::for_each(boxes,[&result,time_diff,&inner_index](auto & foo) {
						result.template block<foo.inner_size,1>(inner_index,0)=(foo.getSTD(time_diff));
						inner_index+=foo.inner_size;
					});
			return result;
		}

		/**
		 * Call normalise function of all state boxes
		 * generally needed for orientation boxes to prevent numeric failures
		 * @param state the current state
		 * @return a normalised version of the state
		 */
		OUTER_T<double> normalise(const OUTER_T<double> & state) const {
			OUTER_T<double> result=result.Zero();
			int outer_index=0;
			boost::hana::for_each(boxes,[&result,&state,&outer_index](auto const & foo) {
						result.template block<foo.outer_size,1>(outer_index,0)=foo.normalise(state.template block<foo.outer_size,1>(outer_index,0));
						outer_index+=foo.outer_size;
					});
			return result;
		}

	};
}

#endif /* ESTIMATORS_MODELS_STATE_HPP_ */


//
// Created by Hamza El-Kebir on 6/18/21.
//

#ifndef LODESTAR_BACKWARDDIFFERENCE_HPP
#define LODESTAR_BACKWARDDIFFERENCE_HPP

#include <Eigen/Dense>

namespace ls {
    namespace primitives {
        template<typename TType, size_t TSamples, size_t TOrder, typename TScalarType = double>
        class BackwardDifference {
            static_assert(TSamples > 1, "Number of samples must be greater than one.");

            static_assert(TOrder > 1, "Differentiation order must be greater than one.");

            static_assert(TSamples > TOrder, "Number of samples must be greater than order.");

            static_assert(
                    // First order
                    (TSamples == 2 && TOrder == 1) ||
                    (TSamples == 3 && TOrder == 1) ||
                    (TSamples == 4 && TOrder == 1) ||
                    (TSamples == 5 && TOrder == 1) ||
                    (TSamples == 6 && TOrder == 1) ||
                    (TSamples == 7 && TOrder == 1) ||
                    // Second order
                    (TSamples == 3 && TOrder == 2) ||
                    (TSamples == 4 && TOrder == 2) ||
                    (TSamples == 5 && TOrder == 2) ||
                    (TSamples == 6 && TOrder == 2) ||
                    (TSamples == 7 && TOrder == 2) ||
                    // Third order
                    (TSamples == 4 && TOrder == 3) ||
                    (TSamples == 5 && TOrder == 3) ||
                    (TSamples == 6 && TOrder == 3) ||
                    (TSamples == 7 && TOrder == 3), "Combination of samples and order is not supported.");
        };

// ------- First derivative ---------

        /**
         * @brief Computes the first-order backward finite difference difference given two samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 2, 1, TScalarType> {
        protected:
            static TType compute(const TType &xNeg1, const TType &x0)
            {
                return x0 - xNeg1;
            }

        public:
            static TType compute(const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 1;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given three samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 3, 1, TScalarType> {
        protected:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (3.0 / 2.0) * x0 - 2.0 * xNeg1 + (1.0 / 2.0) * xNeg2;
            }

        public:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 2;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given four samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 4, 1, TScalarType> {
        protected:
            static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (11.0 / 6.0) * x0 - 3.0 * xNeg1 + (3.0 / 2.0) * xNeg2 - (1.0 / 3.0) * xNeg3;
            }

        public:
            static TType
            compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 3;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given five samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 5, 1, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (25.0 / 12.0) * x0 - 4.0 * xNeg1 + 3.0 * xNeg2 - (4.0 / 3.0) * xNeg3 + (1.0 / 4.0) * xNeg4;
            }

        public:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
                    TScalarType h)
            {
                return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 4;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given six samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 6, 1, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0)
            {
                return (137.0 / 60.0) * x0 - 5.0 * xNeg1 + 5.0 * xNeg2 - (10.0 / 3.0) * xNeg3 + (5.0 / 4.0) * xNeg4 -
                       (1.0 / 6.0) * xNeg5;
            }

        public:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0, TScalarType h)
            {
                return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 5;
        };

        /**
         * @brief Computes the first-order backward finite difference difference given seven samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 7, 1, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0)
            {
                return (49.0 / 20.0) * x0 - 6.0 * xNeg1 + (15.0 / 2.0) * xNeg2 - (20.0 / 3.0) * xNeg3 +
                       (15.0 / 4.0) * xNeg4 - (6.0 / 5.0) * xNeg5 + (1.0 / 6.0) * xNeg6;
            }

        public:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
            }

            static const int kOrder = 1;
            static const int kErrorOrder = 6;
        };

// ------- Second derivative ---------

        /**
         * @brief Computes the second-order backward finite difference difference given three samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 3, 2, TScalarType> {
        protected:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return x0 - 2 * xNeg1 + xNeg2;
            }

        public:
            static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 1;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given four samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 4, 2, TScalarType> {
        protected:
            static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return 2 * x0 - 5 * xNeg1 + 4 * xNeg2 - 1 * xNeg3;
            }

        public:
            static TType
            compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 2;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given five samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 5, 2, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (35.0 / 12.0) * x0 - (26.0 / 3.0) * xNeg1 + (19.0 / 2.0) * xNeg2 - (14.0 / 3.0) * xNeg3 +
                       (11.0 / 12.0) * xNeg4;
            }

        public:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
                    TScalarType h)
            {
                return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 3;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given six samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 6, 2, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0)
            {
                return (15.0 / 4.0) * x0 - (77.0 / 6.0) * xNeg1 + (107.0 / 6.0) * xNeg2 - 13.0 * xNeg3 +
                       (61.0 / 12.0) * xNeg4 - (5.0 / 6.0) * xNeg5;
            }

        public:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0, TScalarType h)
            {
                return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 4;
        };

        /**
         * @brief Computes the second-order backward finite difference difference given seven samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 7, 2, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0)
            {
                return (203.0 / 45.0) * x0 - (87.0 / 5.0) * xNeg1 + (117.0 / 4.0) * xNeg2 - (254.0 / 9.0) * xNeg3 +
                       (33.0 / 2.0) * xNeg4 - (27.0 / 5.0) * xNeg5 + (137.0 / 180.0) * xNeg6;
            }

        public:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
            }

            static const int kOrder = 2;
            static const int kErrorOrder = 5;
        };

// ------- Third derivative ---------

        /**
         * @brief Computes the third-order backward finite difference difference given four samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 4, 3, TScalarType> {
        protected:
            static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return 1.0 * x0 - 3.0 * xNeg1 + 3.0 * xNeg2 - 1.0 * xNeg3;
            }

        public:
            static TType
            compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 1;
        };

        /**
         * @brief Computes the third-order backward finite difference difference given five samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 5, 3, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
            {
                return (5.0 / 2.0) * x0 - 9.0 * xNeg1 + 12.0 * xNeg2 - 7.0 * xNeg3 + (3.0 / 2.0) * xNeg4;
            }

        public:
            static TType
            compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
                    TScalarType h)
            {
                return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 2;
        };

        /**
         * @brief Computes the third-order backward finite difference difference given six samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 6, 3, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0)
            {
                return (17.0 / 4.0) * x0 - (71.0 / 4.0) * xNeg1 + (59.0 / 2.0) * xNeg2 - (49.0 / 2.0) * xNeg3 +
                       (41.0 / 4.0) * xNeg4 - (7.0 / 4.0) * xNeg5;
            }

        public:
            static TType
            compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
                    const TType &x0, TScalarType h)
            {
                return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 3;
        };

        /**
         * @brief Computes the third-order backward finite difference difference given seven samples.
         *
         * @tparam TType State type.
         * @tparam TScalarType Spacing type.
         */
        template<typename TType, typename TScalarType>
        class BackwardDifference<TType, 7, 3, TScalarType> {
        protected:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0)
            {
                return (49.0 / 8.0) * x0 - 29.0 * xNeg1 + (461.0 / 8.0) * xNeg2 - 62.0 * xNeg3 + (307.0 / 8.0) * xNeg4 -
                       13.0 * xNeg5 + (15.0 / 8.0) * xNeg6;
            }

        public:
            static TType
            compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
                    const TType &xNeg1, const TType &x0, TScalarType h)
            {
                return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
            }

            static const int kOrder = 3;
            static const int kErrorOrder = 4;
        };

    }
}

#endif //LODESTAR_BACKWARDDIFFERENCE_HPP


#include <iostream>
#include "linop.h"
#include "utils.h"
#include "math.h"
#include <Eigen/Cholesky>

using namespace Rcpp;
using namespace RcppEigen;

RcppExport SEXP gls(SEXP X, SEXP S, SEXP Y, SEXP maxit, SEXP tol)
{
  using namespace Rcpp;
  using namespace RcppEigen;
  try {
    using Eigen::Map;
    using Eigen::MatrixXd;
    using Eigen::VectorXd;
    using Rcpp::List;

    typedef Map<VectorXd> MapVecd;
    typedef Map<Eigen::MatrixXd> MapMatd;

    const int maxiter(as<int>(maxit));
    const double toler(as<double>(tol));
    const Eigen::Map<MatrixXd> XX(as<MapMatd>(X));
    const Eigen::Map<MatrixXd> SS(as<MapMatd>(S));
    const Eigen::Map<VectorXd> YY(as<MapVecd>(Y));

    const int n(XX.rows());
    const int p(XX.cols());

    MatrixXd SX(MatrixXd(n, p));
    //MatrixXd XSX(MatrixXd(p, p));

    for (int i = 0; i < p; i++) {
      SX.col(i) = conjugate_gradient(SS, XX.col(i), maxiter, toler);
    }

    MatrixXd XSX((XX.adjoint()) * SX);
    VectorXd XSY((SX.adjoint()) * YY);

    VectorXd beta = conjugate_gradient(XSX, XSY, maxiter, toler);

    return List::create(Named("beta") = beta);
  } catch (std::exception &ex) {
    forward_exception_to_r(ex);
  } catch (...) {
    ::Rf_error("C++ exception (unknown reason)");
  }
  return R_NilValue; //-Wall
}

RcppExport SEXP gls_half_cg(SEXP X, SEXP S, SEXP Y, SEXP maxit, SEXP tol)
{
  using namespace Rcpp;
  using namespace RcppEigen;
  try {
    using Eigen::Map;
    using Eigen::MatrixXd;
    using Eigen::VectorXd;

    using Rcpp::List;

    typedef Map<VectorXd> MapVecd;
    typedef Map<Eigen::MatrixXd> MapMatd;

    const int maxiter(as<int>(maxit));
    const double toler(as<double>(tol));
    const Eigen::Map<MatrixXd> XX(as<MapMatd>(X));
    const Eigen::Map<MatrixXd> SS(as<MapMatd>(S));
    const Eigen::Map<VectorXd> YY(as<MapVecd>(Y));

    const int n(XX.rows());
    const int p(XX.cols());

    MatrixXd SX(MatrixXd(n, p));

    SX = SS.llt().solve(XX);

    MatrixXd XSX((XX.adjoint()) * SX);
    VectorXd XSY((SX.adjoint()) * YY);

    VectorXd beta = conjugate_gradient(XSX, XSY, maxiter, toler);

    return List::create(Named("beta") = beta);
  } catch (std::exception &ex) {
    forward_exception_to_r(ex);
  } catch (...) {
    ::Rf_error("C++ exception (unknown reason)");
  }
  return R_NilValue; //-Wall
}

RcppExport SEXP gls_direct(SEXP X, SEXP S, SEXP Y, SEXP maxit, SEXP tol)
{
  using namespace Rcpp;
  using namespace RcppEigen;
  try {
    using Eigen::Map;
    using Eigen::MatrixXd;
    using Eigen::VectorXd;

    using Rcpp::List;

    typedef Map<VectorXd> MapVecd;
    typedef Map<Eigen::MatrixXd> MapMatd;

    const int maxiter(as<int>(maxit));
    const double toler(as<double>(tol));
    const Eigen::Map<MatrixXd> XX(as<MapMatd>(X));
    const Eigen::Map<MatrixXd> SS(as<MapMatd>(S));
    const Eigen::Map<VectorXd> YY(as<MapVecd>(Y));

    const int n(XX.rows());
    const int p(XX.cols());

    MatrixXd SX(MatrixXd(n, p));

    SX = SS.llt().solve(XX);

    MatrixXd XSX((XX.adjoint()) * SX);
    VectorXd XSY((SX.adjoint()) * YY);

    VectorXd beta = XSX.ldlt().solve(XSY);

    return List::create(Named("beta") = beta);
  } catch (std::exception &ex) {
    forward_exception_to_r(ex);
  } catch (...) {
    ::Rf_error("C++ exception (unknown reason)");
  }
  return R_NilValue; //-Wall
}


#define CATCH_CONFIG_MAIN

#include "CALPHADConcSolverBinaryThreePhase.h"
#include "CALPHADFreeEnergyFunctionsBinary.h"
#include "CALPHADFreeEnergyFunctionsBinaryThreePhase.h"

#include "InterpolationType.h"
#include "PhysicalConstants.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <fstream>
#include <iostream>
#include <string>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD conc solver binary three phase KKS, two-phase consistancy",
    "[conc solver binary three phase kks, two-phase consistancy]")
{
    // Calculate the inputs and the reference solution
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 1450.;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadAuNi.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // Get the CALPHAD parameters
    CalphadDataType fA[2];
    CalphadDataType fB[2];
    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];

    cafe.computeTdependentParameters(temperature, Lmix_L, Lmix_A, fA, fB);

    // initial guesses
    double c_init0 = 0.5;
    double c_init1 = 0.5;

    double sol_reference[2] = { c_init0, c_init1 };

    // compute concentrations satisfying KKS equations
    const double conc = 0.3;
    const double phi  = 0.5;
    cafe.computePhaseConcentrations(
        temperature, &conc, &phi, &sol_reference[0]);

    // Inputs to the solver
    const double RTinv
        = 1.0 / (Thermo4PFM::gas_constant_R_JpKpmol * temperature);

    double hphi0 = interp_func(conc_interp_func_type, 1.0 - phi);
    double hphi1 = interp_func(conc_interp_func_type, phi);
    double hphi2 = 0.0;

    CalphadDataType Lmix_S0[4];
    CalphadDataType Lmix_S1[4];
    for (int i = 0; i < 4; ++i)
    {
        Lmix_S0[i] = Lmix_A[i];
        Lmix_S1[i] = Lmix_A[i];
    }

    const double tol    = 1.e-8;
    const double alpha  = 0.5; // Using alpha=1 can lead to convergence issues
    const int max_iters = 100;

    // Create and set up the solver
    CalphadDataType fA_threePhase[3];
    CalphadDataType fB_threePhase[3];
    fA_threePhase[0] = fA[0];
    fA_threePhase[1] = fA[1];
    fA_threePhase[2] = fA[1];
    fB_threePhase[0] = fB[0];
    fB_threePhase[1] = fB[1];
    fB_threePhase[2] = fB[1];

    Thermo4PFM::CALPHADConcSolverBinaryThreePhase solver;
    solver.setup(conc, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_S0, Lmix_S1,
        fA_threePhase, fB_threePhase);

    // Run the solver
    double sol_test[3] = { c_init0, c_init1, 1.0e-8 };
    std::cout << "First test..." << std::endl;
    int ret = solver.ComputeConcentration(sol_test, tol, max_iters, alpha);
    std::cout << "...completed" << std::endl;
    REQUIRE(ret >= 0);

    // Plug the solution back into the RHS
    double residual[3];
    solver.RHS(sol_test, residual);

    REQUIRE(std::abs(residual[0]) < 1.1 * tol);
    REQUIRE(std::abs(residual[1]) < 1.1 * tol);
    REQUIRE(std::abs(residual[2]) < 1.1 * tol);

    // Make sure that it matches the binary case
    REQUIRE(std::abs(sol_test[0] - sol_reference[0]) < 1.1 * tol);
    REQUIRE(std::abs(sol_test[1] - sol_reference[1]) < 1.1 * tol);

    // ----------
    // Now do the same but for the second solid phase
    hphi0 = interp_func(conc_interp_func_type, 1.0 - phi);
    hphi1 = 0.0;
    hphi2 = interp_func(conc_interp_func_type, phi);

    Thermo4PFM::CALPHADConcSolverBinaryThreePhase solver2;
    solver2.setup(conc, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_S0, Lmix_S1,
        fA_threePhase, fB_threePhase);

    // Run the solver
    sol_test[0] = c_init0;
    sol_test[1] = 1.0e-8;
    sol_test[2] = c_init1;

    residual[0] = 0.;
    residual[1] = 0.;
    residual[2] = 0.;

    std::cout << "Second test..." << std::endl;
    ret = solver2.ComputeConcentration(sol_test, tol, max_iters, alpha);
    std::cout << "...completed" << std::endl;
    REQUIRE(ret >= 0);

    // Plug the solution back into the RHS
    solver2.RHS(sol_test, residual);

    REQUIRE(std::abs(residual[0]) < 1.1 * tol);
    REQUIRE(std::abs(residual[1]) < 1.1 * tol);
    REQUIRE(std::abs(residual[2]) < 1.1 * tol);

    // Make sure that it matches the binary case
    REQUIRE(std::abs(sol_test[0] - sol_reference[0]) < 2.0 * tol);
    REQUIRE(std::abs(sol_test[2] - sol_reference[1]) < 2.0 * tol);
}

TEST_CASE("CALPHAD conc solver binary three phase KKS, three-phase convergence",
    "[conc solver binary three phase kks, three-phase convergence]")
{
    // Calculate the inputs and the reference solution
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 900.;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json(
            "../thermodynamic_data/calphadAlCuLFccBcc.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinaryThreePhase cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // Get the CALPHAD parameters
    CalphadDataType fA[3];
    CalphadDataType fB[3];
    CalphadDataType Lmix_L[4];
    CalphadDataType Lmix_A[4];
    CalphadDataType Lmix_B[4];

    cafe.computeTdependentParameters(
        temperature, Lmix_L, Lmix_A, Lmix_B, fA, fB);

    // initial guesses
    double c_init0 = 0.9;
    double c_init1 = 0.9;
    double c_init2 = 0.9;

    // compute concentrations satisfying KKS equations
    const double conc = 0.9;

    // Inputs to the solver
    const double RTinv
        = 1.0 / (Thermo4PFM::gas_constant_R_JpKpmol * temperature);

    double hphi0 = interp_func(conc_interp_func_type, 0.5);
    double hphi1 = interp_func(conc_interp_func_type, 0.4);
    double hphi2 = interp_func(conc_interp_func_type, 0.1);

    const double tol    = 1.e-8;
    const double alpha  = 0.1; // Using alpha=1 can lead to convergence issues
    const int max_iters = 10000;

    // Create and set up the solver

    Thermo4PFM::CALPHADConcSolverBinaryThreePhase solver;
    solver.setup(
        conc, hphi0, hphi1, hphi2, RTinv, Lmix_L, Lmix_A, Lmix_B, fA, fB);

    // Run the solver
    double sol_test[3] = { c_init0, c_init1, c_init2 };
    std::cout << "Third test..." << std::endl;
    int ret = solver.ComputeConcentration(sol_test, tol, max_iters, alpha);
    std::cout << "...completed" << std::endl;
    std::cout << sol_test[0] << " " << sol_test[1] << " " << sol_test[2]
              << std::endl;
    REQUIRE(ret >= 0);

    // Plug the solution back into the RHS
    double residual[3];
    solver.RHS(sol_test, residual);

    REQUIRE(std::abs(residual[0]) < 1.1 * tol);
    REQUIRE(std::abs(residual[1]) < 1.1 * tol);
    REQUIRE(std::abs(residual[2]) < 1.1 * tol);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_SQRTVALMAX_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_SQRTVALMAX_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Sqrtvalmax Sqrtvalmax (function template)

  Generates the square root of the greatest finite representable value.

  @headerref{<boost/simd/constant/sqrtvalmax.hpp>}

  @par Description

  1.  @code
      template<typename T> T Sqrtvalmax();
      @endcode

  2.  @code
      template<typename T> T Sqrtvalmax( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to the value of the greatest representable value
  which square is also representable.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `sqrt(Valmax<T>())`

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/sqrtvalmax.hpp>
#include <boost/simd/constant/simd/sqrtvalmax.hpp>

#endif


/*
 * Filename: markers.cpp
 *
 * Copyright 2020 Tecnalia
 *
 * 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 <manipulability_metrics/util/markers.h>

#include <eigen_conversions/eigen_kdl.h>
#include <eigen_conversions/eigen_msg.h>

#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <algorithm>

namespace manipulability_metrics
{
visualization_msgs::Marker ellipsoidMarker(const Eigen::Matrix<double, 3, Eigen::Dynamic>& half_jacobian,
                                           const Eigen::Isometry3d& transform, const std::string& root,
                                           const std::string& ns, const std_msgs::ColorRGBA& color)
{
  auto svd = Eigen::JacobiSVD<Eigen::Matrix<double, 3, Eigen::Dynamic>>{ half_jacobian, Eigen::ComputeFullU };
  auto base = svd.matrixU();
  auto len = static_cast<Eigen::Vector3d>(svd.singularValues().topRows<3>());
  if (base.determinant() < 0)
  {
    base.col(1).swap(base.col(2));
    len.row(1).swap(len.row(2));
  }

  auto ellipsoid_marker = visualization_msgs::Marker{};
  ellipsoid_marker.type = visualization_msgs::Marker::SPHERE;
  ellipsoid_marker.header.frame_id = root;
  ellipsoid_marker.ns = ns;
  ellipsoid_marker.color = color;
  tf::pointEigenToMsg(transform.translation(), ellipsoid_marker.pose.position);
  tf::quaternionEigenToMsg(static_cast<Eigen::Quaterniond>(base), ellipsoid_marker.pose.orientation);
  tf::vectorEigenToMsg(len, ellipsoid_marker.scale);

  return ellipsoid_marker;
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const std::vector<double>& joint_positions,
                                                 const std::string& ns_prefix)
{
  return ellipsoidMarkers(chain, KDL::Frame{}, joint_positions, ns_prefix);
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const KDL::JntArray& joint_positions,
                                                 const std::string& ns_prefix)
{
  return ellipsoidMarkers(chain, KDL::Frame{}, joint_positions, ns_prefix);
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const KDL::Frame& tcp,
                                                 const std::vector<double>& joint_positions,
                                                 const std::string& ns_prefix)
{
  auto jnt_array = KDL::JntArray{ static_cast<unsigned int>(joint_positions.size()) };
  jnt_array.data =
      Eigen::Map<const Eigen::VectorXd>{ joint_positions.data(), static_cast<Eigen::Index>(joint_positions.size()) };
  return ellipsoidMarkers(chain, tcp, jnt_array, ns_prefix);
}

visualization_msgs::MarkerArray ellipsoidMarkers(const Chain& chain, const KDL::Frame& tcp,
                                                 const KDL::JntArray& joint_positions, const std::string& ns_prefix)
{
  auto frame = KDL::Frame{};
  auto jacobian = KDL::Jacobian{ static_cast<unsigned int>(chain.n_joints) };
  if (!chain.transform(joint_positions, tcp, frame) || !chain.jacobian(joint_positions, tcp.p, jacobian))
  {
    return {};
  }

  const auto transform = [&] {
    auto transform = Eigen::Isometry3d{};
    tf::transformKDLToEigen(frame, transform);
    return transform;
  }();

  auto marker_array = visualization_msgs::MarkerArray{};
  marker_array.markers.reserve(2);
  {
    std_msgs::ColorRGBA transparent_green;
    transparent_green.g = 1.0;
    transparent_green.a = 0.5;
    marker_array.markers.push_back(ellipsoidMarker(jacobian.data.topRows<3>(), transform, chain.root,
                                                   ns_prefix + "translational_ellipsoid", transparent_green));
  }
  {
    std_msgs::ColorRGBA transparent_red;
    transparent_red.r = 1.0;
    transparent_red.a = 0.5;
    marker_array.markers.push_back(ellipsoidMarker(jacobian.data.bottomRows<3>(), transform, chain.root,
                                                   ns_prefix + "rotational_ellipsoid", transparent_red));
  }

  return marker_array;
}

visualization_msgs::MarkerArray desiredEllipsoidMarkers(const Ellipsoid& desired_ellipsoid, const std::string& frame_id,
                                                        const KDL::Frame& transform)
{
  auto pseudoU = Eigen::Matrix<double, 6, 6>{};
  auto pseudoS = Eigen::Matrix<double, 6, 6>{ Eigen::Matrix<double, 6, 6>::Zero() };
  for (std::size_t i = 0; i < 6; ++i)
  {
    pseudoU.col(i) = desired_ellipsoid[i].unit;
    pseudoS(i, i) = desired_ellipsoid[i].len;
  }
  const auto pseudojac = pseudoU * pseudoS;

  const auto eigen_transform = [&] {
    auto eigen_transform = Eigen::Isometry3d{};
    tf::transformKDLToEigen(transform, eigen_transform);
    return eigen_transform;
  }();

  auto markers = visualization_msgs::MarkerArray{};
  markers.markers.reserve(2);
  {
    std_msgs::ColorRGBA transparent_light_green;
    transparent_light_green.r = 0.2;
    transparent_light_green.b = 0.2;
    transparent_light_green.g = 1.0;
    transparent_light_green.a = 0.5;

    markers.markers.push_back(ellipsoidMarker(pseudojac.topRows<3>(), eigen_transform, frame_id,
                                              "desired_translational_ellipsoid", transparent_light_green));
  }
  {
    std_msgs::ColorRGBA transparent_light_red;
    transparent_light_red.r = 1.0;
    transparent_light_red.b = 0.2;
    transparent_light_red.g = 0.2;
    transparent_light_red.a = 0.5;

    markers.markers.push_back(ellipsoidMarker(pseudojac.bottomRows<3>(), eigen_transform, frame_id,
                                              "desired_rotational_ellipsoid", transparent_light_red));
  }
  return markers;
}

}  // namespace manipulability_metrics


#include <stan/math/prim/mat.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathMatrix,dimensionValidation) {
  using stan::math::determinant;
  using Eigen::Matrix;
  using Eigen::Dynamic;
  Matrix<double,Dynamic,Dynamic> x(3,3);
  x << 1, 2, 3, 1, 4, 9, 1, 8, 27;

  ASSERT_FALSE(boost::math::isnan(determinant(x)));

  Matrix<double,Dynamic,Dynamic> xx(3,2);
  xx << 1, 2, 3, 1, 4, 9;
  EXPECT_THROW(stan::math::determinant(xx), std::invalid_argument);
}


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

///
/// \file fast_esprit.hpp
///
#ifndef MXPFIT_FAST_ESPRIT_HPP
#define MXPFIT_FAST_ESPRIT_HPP

#include <algorithm>
#include <type_traits>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <mxpfit/exponential_sum.hpp>
#include <mxpfit/hankel_matrix.hpp>
#include <mxpfit/partial_lanczos_bidiagonalization.hpp>

// #include <mxpfit/matrix_free_gemv.hpp>
// #include <mxpfit/vandermonde_least_squares.hpp>
#include <mxpfit/prony_like_method_common.hpp>

namespace mxpfit
{
///
/// ### FastESPRIT
///
/// \brief Fast ESPRIT method for finding parameters of exponential sum
/// approximation from sampled data on uniform grid.
///
/// \tparam T  Scalar type of function values.
///
/// #### Description
///
/// For a given sequence \f$ f_{k}=f(t_{k}) \f$ sampled on a uniform grid
/// \f$t_{k}=t_{0}+hk\, (k=0,1,\dots,N-1)\f$ and prescribed accuracy
/// \f$\epsilon\f$, this class finds exponential sum approximation of function
/// \f$ f(t) \f$ such that,
///
/// \f[
///   \left| f_{k}-\sum_{j=1}^{M}c_{j}e^{-a_{j} t} \right| < \epsilon
/// \f]
///
/// with \f$\mathrm{Re}(a_{j}) > 0.\f$ The problem is solved using the fast
/// ESPRIT algorithm via partial Lanczos bidiagonalization which has been
/// developed by Potts and Tasche (2015). The present algorithm was slightly
/// modified from the original one.
///
/// #### References
///
/// 1. D. Potts and M. Tasche, "Fast ESPRIT algorithms based on partial singular
///    value decompositions", Appl. Numer. Math. **88** (2015) 31-45.
///    [DOI: https://doi.org/10.1016/j.apnum.2014.10.003]
/// 2. D. Potts and M. Tasche,"Parameter estimation for nonincreasing
///    exponential sums by Prony-like methods", Linear Algebra Appl. **439**
///    (2013) 1024-1039.
///    [DOI: https://doi.org/10.1016/j.laa.2012.10.036]
/// 3. K. Browne, S. Qiao, and Y. Wei, "A Lanczos bidiagonalization algorithm
///    for Hankel matrices", Linear Algebra Appl. **430** (2009) 1531-1543.
///    [DOI: https://doi.org/10.1016/j.laa.2008.01.012]
///
template <typename T>
class FastESPRIT
{
public:
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using Index         = Eigen::Index;

    using Vector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVector    = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using ComplexVector = Eigen::Matrix<ComplexScalar, Eigen::Dynamic, 1>;

    using Matrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using RealMatrix =
        Eigen::Matrix<RealScalar, Eigen::Dynamic, Eigen::Dynamic>;
    using ComplexMatrix =
        Eigen::Matrix<ComplexScalar, Eigen::Dynamic, Eigen::Dynamic>;

    using ResultType =
        typename detail::gen_prony_like_method_result<T>::ResultType;

private:
    using HankelGEMV      = MatrixFreeGEMV<HankelMatrix<Scalar>>;
    using VandermondeGEMV = MatrixFreeGEMV<VandermondeMatrix<ComplexScalar>>;
    enum
    {
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex,
        Alignment = Eigen::internal::traits<Matrix>::Alignment
    };

    using MappedMatrix = Eigen::Map<Matrix, Alignment>;

    Index m_rows;
    Index m_cols;
    Index m_max_terms;
    Index m_extra_basis = 5;
    HankelMatrix<Scalar> m_matH;
    PartialLanczosBidiagonalization<HankelGEMV> m_plbd;

public:
    ///
    /// Default constructor
    ///
    FastESPRIT() = default;

    ///
    /// Constructor with memory preallocation
    ///
    /// \param[in] N  Number of sampling points
    /// \param[in] L  Window size. This is equals to the number of rows of
    ///               generalized Hankel matrix.
    /// \param[in] M  Maxumum number of terms used for the exponential sum.
    /// \pre  `N >= M >= 1` and `N - L + 1 >= M >= 1`.
    ///
    FastESPRIT(Index N, Index L, Index M)
        : m_rows(L),
          m_cols(N - L + 1),
          m_max_terms(M),
          m_extra_basis(5),
          m_matH(m_rows, m_cols),
          m_plbd(m_rows, m_cols,
                 std::min({m_rows, m_cols, m_max_terms + m_extra_basis}))
    {
        assert(m_rows >= M && m_cols >= M && M >= 1);
    }

    /// Destructor
    ~FastESPRIT()
    {
    }

    ///
    /// Memory reallocation
    ///
    /// \param[in] N  Number of sampling points
    /// \param[in] L  Window size. This is equals to the number of rows of
    ///               generalized Hankel matrix.
    /// \param[in] M  Maxumum number of terms used for the exponential sum.
    /// \pre  `N >= L >= N / 2 >= 1` and `N - L + 1 >= M >= 1`.
    ///
    void resize(Index N, Index L, Index M)
    {
        m_rows = L;
        m_cols = N - L + 1;
        assert(m_rows >= M && m_cols >= M && M >= 1);
        m_max_terms = M;
        m_matH.resize(m_rows, m_cols);
        // Internal matrices of `m_plbd` are resized automatically during
        // computation
    }

    ///
    /// \return Number of sampling points.
    ///
    Index size() const
    {
        return m_matH.size();
    }

    ///
    /// Fit signals by a exponential sum
    ///
    /// \param[in] f The array of signals sampled on the equispaced grid. The
    ///    first `size()` elemnets of `f` are used as a sampled data. In case
    ///    `f.size() < size()` then, last `size() - f.size()` elements are
    ///    padded by zeros.
    /// \param[in] eps  Small positive number `(0 < eps < 1)` that
    ///    controlls the accuracy of the fit.
    /// \param[in] x0  Argument of first sampling point
    /// \param[in] delta Spacing between neighboring sample points.
    ///
    template <typename VectorT>
    ResultType compute(const Eigen::MatrixBase<VectorT>& h, RealScalar x0,
                       RealScalar delta, RealScalar eps);
};

template <typename T>
template <typename VectorT>
typename FastESPRIT<T>::ResultType
FastESPRIT<T>::compute(const Eigen::MatrixBase<VectorT>& h, RealScalar x0,
                       RealScalar delta, RealScalar eps)
{
    using Eigen::numext::sqrt;
    assert(h.size() == size() && "Number of data points mismatch.");
    //
    // Form rectangular Hankel matrix and pre-compute for fast multiplication to
    // the vector.
    //
    m_matH.setCoeffs(h);
    const Index nr = m_matH.rows();
    const Index nc = m_matH.cols();

    //-------------------------------------------------------------------------
    // Compute roots of Prony polynomials
    //-------------------------------------------------------------------------
    //
    // Partial Lanczos bidiagonalization of Hankel matrix H = P B Q^H
    //
    HankelGEMV opH(m_matH);
    m_plbd.setTolerance(eps);
    m_plbd.compute(opH, std::min({nr, nc, m_max_terms + m_extra_basis}));
    const Index nterms = m_plbd.rank();

    if (nterms == 0)
    {
        return ResultType();
    }

    // {
    //     Matrix denseH = m_matH.toDenseMatrix();
    //     std::cout << "Partial LBD error: ||H - P B Q*||_F  = "
    //               << (denseH - m_plbd.reconstructedMatrix()).norm()
    //               << std::endl;
    //     std::cout << "(alpha):\n"
    //               << m_plbd.diagonalAlpha().head(nterms) << "\n(beta):\n"
    //               << m_plbd.superdiagonalBeta().head(nterms - 1) <<
    //               std::endl;
    // }

    // --- Form the views of matrix Q
    // Matrix Q excluding the last row
    auto Q0 = m_plbd.matrixQ().block(0, 0, nc - 1, nterms);
    // Matrix Q excluding the first row
    auto Q1 = m_plbd.matrixQ().block(1, 0, nc - 1, nterms);
    // adjoint of the last row of matrix Q
    auto nu = m_plbd.matrixQ().block(nc - 1, 0, 1, nterms).adjoint();
    //
    // Compute the spectral matrix G = pinv(Q0) * Q1, where pinv indicate
    // the Moore-Penrose pseudo-inverse. The computation of the
    // pseudo-inverse of Q0 can be avoided.
    //
    Matrix G(Q0.adjoint() * Q1);
    Vector phi(G.adjoint() * nu);
    auto scal = RealScalar(1) / (RealScalar(1) - nu.squaredNorm());
    G += scal * nu * phi.adjoint();
    //
    // Prony roots \f$\{z_i\{\}\f$ are the eigenvalues of matrix G.
    // The exponents for approximation are obtained as \f$ \log z_i \f$
    //
    ComplexVector tmp_roots(G.eigenvalues());
    // std::cout << "\n*** roots\n" << tmp_roots << std::endl;
    //
    // Find roots on unit disk \f$z_{i} \in \mathbb{D}\f$.
    // If Scalar is a real type, neagative real roots will also be discarded.
    //
    tmp_roots =
        detail::prony_roots_on_unit_disk<Scalar>::compute(tmp_roots, sqrt(eps));

    ComplexVector roots(std::min(tmp_roots.size(), m_max_terms));
    roots = tmp_roots.head(roots.size());

    //----------------------------------------------------------------------
    // Solve overdetermined Vandermonde system to obtain the weights
    //----------------------------------------------------------------------
    ComplexVector weights(roots.size());
    detail::solve_overdetermined_vandermonde(
        roots, h, weights, eps, std::max(Index(100), 5 * roots.size()));

    // Rescale the exponents and weights, and return the result
    ResultType ret(roots.size());
    ret.exponents() = -roots.array().log() / delta;
    if (x0 == RealScalar())
    {
        ret.weights() = weights.array(); // Don't forget to copy weights
    }
    else
    {
        ret.weights() = weights.array() * (-x0 * ret.exponents()).exp();
    }

    return ret;

    // return detail::gen_prony_like_method_result<T>::create(
    //     roots.array(), weights.array(), x0, delta);
}

} // namespace mxpfit

#endif /* MXPFIT_FAST_ESPRIT_HPP */


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testRot3.cpp
 * @brief   Unit tests for Rot3Q class
 * @author  Richard Roberts
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <boost/math/constants/constants.hpp>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>

using namespace gtsam;

typedef BetweenFactor<Rot3> Between;
typedef NonlinearFactorGraph Graph;

/* ************************************************************************* */
TEST(Rot3, optimize) {

  // Optimize a circle
  Values truth;
  Values initial;
  Graph fg;
  fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
  for(int j=0; j<6; ++j) {
    truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
    initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
    fg += Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01));
  }

  Values final = GaussNewtonOptimizer(fg, initial).optimize();

  EXPECT(assert_equal(truth, final, 1e-5));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */