/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "niftkPointUtils.h"
#include <mitkCommon.h>
#include <cmath>
#include <boost/math/special_functions/fpclassify.hpp>

namespace niftk
{

//-----------------------------------------------------------------------------
double CalculateStepSize(double *spacing)
{
  double stepSize = 0;
  double smallestDimension = std::numeric_limits<double>::max();

  for (int i = 0; i< 3; i++)
  {
    if (spacing[i] < smallestDimension)
    {
      smallestDimension = spacing[i];
    }
  }
  stepSize = smallestDimension / 3.0;
  return stepSize;
}

//-----------------------------------------------------------------------------
bool AreDifferent(const mitk::Point3D& a, const mitk::Point3D& b)
{
  bool areDifferent = false;

  for (int i = 0; i < 3; i++)
  {
    if (fabs(a[i] - b[i]) > 0.01)
    {
      areDifferent = true;
      break;
    }
  }

  return areDifferent;
}

//-----------------------------------------------------------------------------
double GetSquaredDistanceBetweenPoints(const mitk::Point3D& a, const mitk::Point3D& b)
{
    double distance = 0;

    for (int i = 0; i < 3; i++)
    {
      distance += (a[i] - b[i])*(a[i] - b[i]);
    }

    return distance;
}

//-----------------------------------------------------------------------------
double GetRMSErrorBetweenPoints(
  const mitk::PointSet& fixedPoints,
  const mitk::PointSet& movingPoints,
  const CoordinateAxesData * const transform)
{
  mitk::PointSet::DataType* itkPointSet = movingPoints.GetPointSet();
  mitk::PointSet::PointsContainer* points = itkPointSet->GetPoints();
  mitk::PointSet::PointsIterator pIt;
  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType fixedPoint;
  mitk::PointSet::PointType movingPoint;
  mitk::PointSet::PointType transformedMovingPoint;

  double rmsError = 0;
  unsigned long int numberOfPointsUsed = 0;

  for (pIt = points->Begin(); pIt != points->End(); ++pIt)
  {
    pointID = pIt->Index();
    movingPoint = movingPoints.GetPoint(pointID);

    if (fixedPoints.GetPointIfExists(pointID, &fixedPoint))
    {
      if (transform != NULL)
      {
        transformedMovingPoint = transform->MultiplyPoint(movingPoint);
        rmsError += GetSquaredDistanceBetweenPoints(fixedPoint, transformedMovingPoint);
      }
      else
      {
        rmsError += GetSquaredDistanceBetweenPoints(fixedPoint, movingPoint);
      }
      numberOfPointsUsed++;
    }
  }
  if (numberOfPointsUsed > 0)
  {
    rmsError /= static_cast<double>(numberOfPointsUsed);
    rmsError = sqrt(rmsError);
  }
  else
  {
    rmsError = 0;
  }
  return rmsError;
}

//-----------------------------------------------------------------------------
void GetDifference(const mitk::Point3D& a, const mitk::Point3D& b, mitk::Point3D& output)
{
  for (int i = 0; i < 3; i++)
  {
    output[i] = a[i] - b[i];
  }
}

//-----------------------------------------------------------------------------
double Length(mitk::Point3D& vector)
{
  double length = 0;
  for (int i = 0; i < 3; i++)
  {
    length += vector[i]*vector[i];
  }
  if (length > 0)
  {
    length = sqrt(length);
  }
  return length;
}

//-----------------------------------------------------------------------------
void Normalise(mitk::Point3D& vector)
{
  double length = Length(vector);
  if (length > 0)
  {
    for (int i = 0; i < 3; i++)
    {
      vector[i] /= length;
    }
  }
}

//-----------------------------------------------------------------------------
double FindLargestDistanceBetweenTwoPoints(const mitk::PointSet& input)
{
  double maxSquaredDistance = 0;

  mitk::PointSet::PointsContainer* inputContainer = input.GetPointSet()->GetPoints();
  mitk::PointSet::PointsConstIterator outerIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator innerIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator iterEnd = inputContainer->End();

  for ( ; outerIt != iterEnd; ++outerIt)
  {
    for ( ; innerIt != iterEnd; ++innerIt)
    {
      double squaredDistance = GetSquaredDistanceBetweenPoints(input.GetPoint(outerIt->Index()), input.GetPoint(innerIt->Index()));
      if (squaredDistance > maxSquaredDistance)
      {
        maxSquaredDistance = squaredDistance;
      }
    }
  }
  return sqrt(maxSquaredDistance);
}

//-----------------------------------------------------------------------------
int CopyPointSets(const mitk::PointSet& input, mitk::PointSet& output)
{
  output.Clear();

  mitk::PointSet::PointsContainer* inputContainer = input.GetPointSet()->GetPoints();
  mitk::PointSet::PointsConstIterator inputIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator inputEnd = inputContainer->End();
  for ( ; inputIt != inputEnd; ++inputIt)
  {
    output.InsertPoint(inputIt->Index(), input.GetPoint(inputIt->Index()));
  }
  return output.GetSize();
}

//-----------------------------------------------------------------------------
void ScalePointSets(const mitk::PointSet& input, mitk::PointSet& output, double scaleFactor)
{
  output.Clear();

  mitk::PointSet::PointsContainer* inputContainer = input.GetPointSet()->GetPoints();
  mitk::PointSet::PointsConstIterator inputIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator inputEnd = inputContainer->End();
  mitk::PointSet::PointType point;

  for ( ; inputIt != inputEnd; ++inputIt)
  {
    point = input.GetPoint(inputIt->Index());
    point[0] *= scaleFactor;
    point[1] *= scaleFactor;
    point[2] *= scaleFactor;

    output.InsertPoint(inputIt->Index(), point);
  }
}

//-----------------------------------------------------------------------------
void CopyValues(const mitk::Point3D& a, mitk::Point3D& b)
{
  for (int i = 0; i < 3; i++)
  {
    b[i] = a[i];
  }
}

//-----------------------------------------------------------------------------
void CrossProduct(const mitk::Point3D& a, const mitk::Point3D& b, mitk::Point3D& c)
{
  mitk::Point3D aCopy;
  mitk::Point3D bCopy;
  CopyValues(a, aCopy);
  CopyValues(b, bCopy);
  Normalise(aCopy);
  Normalise(bCopy);

  c[0] = aCopy[1]*bCopy[2] - bCopy[1]*aCopy[2];
  c[1] = -1 * (aCopy[0]*bCopy[2] - bCopy[0]*aCopy[2]);
  c[2] = aCopy[0]*bCopy[1] - bCopy[0]*aCopy[1];
}

//-----------------------------------------------------------------------------
void ComputeNormalFromPoints(const mitk::Point3D& a, const mitk::Point3D& b, const mitk::Point3D& c, mitk::Point3D& output)
{
  mitk::Point3D aMinusB;
  mitk::Point3D cMinusB;
  GetDifference(a, b, aMinusB);
  GetDifference(c, b, cMinusB);
  CrossProduct(aMinusB, cMinusB, output);
  Normalise(output);
}

//-----------------------------------------------------------------------------
void TransformPointByVtkMatrix(
    const vtkMatrix4x4* matrix,
    const bool& isNormal,
    mitk::Point3D& point
    )
{
  double transformedPoint[4] = {0, 0, 0, 1};
  vtkMatrix4x4* nonConstMatrix = const_cast<vtkMatrix4x4*>(matrix);

  if(nonConstMatrix != NULL)
  {
    transformedPoint[0] = point[0];
    transformedPoint[1] = point[1];
    transformedPoint[2] = point[2];
    transformedPoint[3] = 1;

    nonConstMatrix->MultiplyPoint(transformedPoint, transformedPoint);

    point[0] = transformedPoint[0];
    point[1] = transformedPoint[1];
    point[2] = transformedPoint[2];

    if (isNormal)
    {
      double transformedOrigin[4] = {0, 0, 0, 1};
      nonConstMatrix->MultiplyPoint(transformedOrigin, transformedOrigin);

      point[0] = point[0] - transformedOrigin[0];
      point[1] = point[1] - transformedOrigin[1];
      point[2] = point[2] - transformedOrigin[2];
    }
  }
}

//-----------------------------------------------------------------------------
void TransformPointsByVtkMatrix(
    const mitk::PointSet& input,
    const vtkMatrix4x4& matrix,
    mitk::PointSet& output
    )
{
  mitk::PointSet::DataType* itkPointSet = input.GetPointSet();
  mitk::PointSet::PointsContainer* points = itkPointSet->GetPoints();
  mitk::PointSet::PointsIterator pIt;
  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType point;

  output.Clear();

  for (pIt = points->Begin(); pIt != points->End(); ++pIt)
  {
    pointID = pIt->Index();
    point = input.GetPoint(pointID);
    TransformPointByVtkMatrix(&matrix, false, point);
    output.InsertPoint(pointID, point);
  }
}

//-----------------------------------------------------------------------------
int FilterMatchingPoints(
    const mitk::PointSet& fixedPointsIn,
    const mitk::PointSet& movingPointsIn,
    mitk::PointSet& fixedPointsOut,
    mitk::PointSet& movingPointsOut
    )
{
  int matchedPoints = 0;
  fixedPointsOut.Clear();
  movingPointsOut.Clear();

  mitk::PointSet::DataType* fixedPointSet = fixedPointsIn.GetPointSet(0);
  mitk::PointSet::PointsContainer* fixedPoints = fixedPointSet->GetPoints();
  mitk::PointSet::DataType* movingPointSet = movingPointsIn.GetPointSet(0);
  mitk::PointSet::PointsContainer* movingPoints = movingPointSet->GetPoints();

  mitk::PointSet::PointsIterator fixedPointsIt;
  mitk::PointSet::PointsIterator movingPointsIt;

  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType fixedPoint;
  mitk::PointSet::PointType movingPoint;

  for (fixedPointsIt = fixedPoints->Begin(); fixedPointsIt != fixedPoints->End(); ++fixedPointsIt)
  {
    pointID = fixedPointsIt->Index();
    fixedPoint = fixedPointsIn.GetPoint(pointID);

    for (movingPointsIt = movingPoints->Begin(); movingPointsIt != movingPoints->End(); ++movingPointsIt)
    {
      if (movingPointsIt->Index() == pointID)
      {
        movingPoint = movingPointsIn.GetPoint(pointID);

        fixedPointsOut.InsertPoint(pointID, fixedPoint);
        movingPointsOut.InsertPoint(pointID, movingPoint);
        matchedPoints++;
      }
    }
  }

  return matchedPoints;
}

//-----------------------------------------------------------------------------
int RemoveNaNPoints(
    const mitk::PointSet& pointsIn,
    mitk::PointSet& pointsOut
    )
{
  int removedPoints = 0;
  pointsOut.Clear();

  mitk::PointSet::DataType* pointSet = pointsIn.GetPointSet(0);
  mitk::PointSet::PointsContainer* points = pointSet->GetPoints();

  mitk::PointSet::PointsIterator pointsIt;

  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType point;

  for (pointsIt = points->Begin(); pointsIt != points->End(); ++pointsIt)
  {
    pointID = pointsIt->Index();
    point = pointsIn.GetPoint(pointID);

    if ( CheckForNaNPoint(point) )
    {
      removedPoints++;
    }
    else
    {
      pointsOut.InsertPoint(pointID, point);
    }
  }
  return removedPoints;
}

//-----------------------------------------------------------------------------
bool CheckForNaNPoint( const mitk::PointSet::PointType& point )
{
  if ( boost::math::isnan( point[0] ) || boost::math::isnan( point[1] ) || boost::math::isnan( point[2] ))
  {
    return true;
  }
  return false;
}

//-----------------------------------------------------------------------------
mitk::Point3D ComputeCentroid(const mitk::PointSet& input)
{
  mitk::Point3D average;
  average.Fill(0);

  if (input.GetSize() > 0)
  {
    mitk::PointSet::DataType* pointSet = input.GetPointSet(0);
    mitk::PointSet::PointsContainer* points = pointSet->GetPoints();
    mitk::PointSet::PointsIterator pointsIt;
    mitk::PointSet::PointType point;

    for (pointsIt = points->Begin(); pointsIt != points->End(); ++pointsIt)
    {
      point = input.GetPoint(pointsIt->Index());
      average[0] += point[0];
      average[1] += point[1];
      average[2] += point[2];
    }

    double numberOfPoints = static_cast<double>(input.GetSize());

    average[0] /= numberOfPoints;
    average[1] /= numberOfPoints;
    average[2] /= numberOfPoints;
  }

  return average;
}

}


#include "RBGL.hpp"
#include <boost/graph/graph_utility.hpp>

using namespace boost;

typedef adjacency_list<vecS, vecS, undirectedS,
	// vertex properties
        property<vertex_index_t, int,
    	property<vertex_centrality_t, double> >,
    	// edge properties
    	property<edge_weight_t, double,
    	property<edge_centrality_t, double> > >
    	BCGraph;

// Explicit instantiation of template function max_element circumvents
// compiler error in llvm8 libc++
typedef graph_traits<BCGraph>::edge_descriptor Edge;
typedef graph_traits<BCGraph>::edge_iterator EdgeIterator;
typedef property_map < BCGraph, edge_centrality_t >::type EdgeCentralityMap;
typedef typename property_traits<EdgeCentralityMap>::value_type centrality_type;
typedef indirect_cmp<EdgeCentralityMap, std::less<centrality_type> > EdgeCentralityCompare;

EdgeIterator
max_element(EdgeIterator __first, EdgeIterator __last, EdgeCentralityCompare __comp)
{
    if (__first != __last)
    {
        EdgeIterator __i = __first;
        while (++__i != __last)
            if (__comp(*__first, *__i))
                __first = __i;
    }
    return __first;
}

#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/betweenness_centrality.hpp>

extern "C"
{

	SEXP BGL_brandes_betweenness_centrality(SEXP num_verts_in,
		SEXP num_edges_in, SEXP R_edges_in, SEXP R_weights_in)
        {
    		BCGraph g;

		int NV = Rf_asInteger(num_verts_in);
		int NE = Rf_asInteger(num_edges_in);
		int* edges_in = INTEGER(R_edges_in);
		double* weights_in = REAL(R_weights_in);

		for (int i = 0; i < NE ; i++, edges_in += 2, weights_in++)
		    boost::add_edge(*edges_in, *(edges_in+1), *weights_in, g);

		SEXP anslst, bcvlst, enlst, bcelst, rbcvlst, dom;
		PROTECT(anslst = Rf_allocVector(VECSXP,5));
		PROTECT(bcvlst = Rf_allocMatrix(REALSXP, 1, NV));
		PROTECT(enlst = Rf_allocMatrix(INTSXP, 2, NE));
		PROTECT(bcelst = Rf_allocMatrix(REALSXP, 1, NE));
		PROTECT(rbcvlst = Rf_allocMatrix(REALSXP, 1, NV));
		PROTECT(dom = Rf_allocVector(REALSXP, 1));

		brandes_betweenness_centrality(g,
                        centrality_map(get(vertex_centrality, g)).
			edge_centrality_map(get(edge_centrality, g)).
			weight_map(get(edge_weight, g)));

                property_map<BCGraph, vertex_centrality_t>::type
			v_map = get(vertex_centrality, g);
                property_map<BCGraph, edge_centrality_t>::type
			e_map = get(edge_centrality, g);

                graph_traits < BCGraph>::vertex_iterator vi, v_end;
                graph_traits < BCGraph>::edge_iterator ei, e_end;

                int v = 0, e = 0;

		for ( tie(vi, v_end) = vertices(g); vi != v_end; vi++ )
                    REAL(bcvlst)[v++] = v_map[*vi];
		for ( v = 0, tie(ei, e_end) = edges(g); ei != e_end ; ei++ )
		{
		    INTEGER(enlst)[v++] = source(*ei, g);
		    INTEGER(enlst)[v++] = target(*ei, g);
                    REAL(bcelst)[e++] = e_map[*ei];
                }

		relative_betweenness_centrality(g, get(vertex_centrality, g));
                v_map = get(vertex_centrality, g);

		for ( v = 0, tie(vi, v_end) = vertices(g); vi != v_end; vi++ )
                    REAL(rbcvlst)[v++] = v_map[*vi];

		double dominance = central_point_dominance(g,
		                  get(vertex_centrality, g));

		REAL(dom)[0] = dominance;

		SET_VECTOR_ELT(anslst,0,bcvlst);
		SET_VECTOR_ELT(anslst,1,bcelst);
		SET_VECTOR_ELT(anslst,2,rbcvlst);
		SET_VECTOR_ELT(anslst,3,dom);
		SET_VECTOR_ELT(anslst,4,enlst);
		UNPROTECT(6);
		return(anslst);
	}

	class clustering_threshold : public bc_clustering_threshold<double>
	{
		typedef bc_clustering_threshold<double> inherited;

	public:
	        clustering_threshold(double threshold, const BCGraph& g, bool normalize)
		: inherited(threshold, g, normalize), iter(1) { }

	        bool operator()(double max_centrality, Edge e, const BCGraph& g)
	        {
#if DEBUG
                  std::cout << "Iter: " << iter << " Max Centrality: "
                       << (max_centrality / dividend) << std::endl;
#endif
		  ++iter;
		  return inherited::operator()(max_centrality, e, g);
	        }

	private:
		 unsigned int iter;
	};

	SEXP BGL_betweenness_centrality_clustering (SEXP num_verts_in,
		SEXP num_edges_in, SEXP R_edges_in, SEXP R_weights_in,
		SEXP R_threshold,  SEXP R_normalize)
        {
    		BCGraph g;

		int NE = Rf_asInteger(num_edges_in);
		int* edges_in = INTEGER(R_edges_in);
		double* weights_in = REAL(R_weights_in);

		for (int i = 0; i < NE ; i++, edges_in += 2, weights_in++)
		    boost::add_edge(*edges_in, *(edges_in+1), *weights_in, g);

		double threshold = REAL(R_threshold)[0];
		bool normalize = LOGICAL(R_normalize)[0];

		betweenness_centrality_clustering(g,
			clustering_threshold(threshold, g, normalize),
			get(edge_centrality, g));

		// betweenness_centrality_clustering(g,
		// 	clustering_threshold(threshold, g, normalize));

		SEXP anslst, cnt, bcvlst, bcelst;
		PROTECT(anslst = Rf_allocVector(VECSXP,3));
		PROTECT(cnt = Rf_allocVector(INTSXP, 1));
		PROTECT(bcvlst = Rf_allocMatrix(INTSXP, 2, num_edges(g)));
		PROTECT(bcelst = Rf_allocMatrix(REALSXP, 1, num_edges(g)));

		INTEGER(cnt)[0] = num_edges(g);

		property_map < BCGraph, edge_centrality_t >::type
		         ec = get(edge_centrality, g);

		typedef graph_traits<BCGraph>::edge_iterator   edge_iterator;
		edge_iterator ei, e_end;

#if DEBUG
                std::cout << " edge centralities: ";
		for ( tie(ei, e_end) = edges(g); ei != e_end; ++ei )
                        std::cout << " " << ec[*ei];
                std::cout << std::endl;
#endif

		int i = 0, j = 0;
		for ( tie(ei, e_end) = edges(g); ei != e_end; ++ei )
		{
			INTEGER(bcvlst)[i++] = source(*ei, g);
			INTEGER(bcvlst)[i++] = target(*ei, g);
			REAL(bcelst)[j++] = ec[*ei];
		}

		SET_VECTOR_ELT(anslst,0,cnt);
		SET_VECTOR_ELT(anslst,1,bcvlst);
		SET_VECTOR_ELT(anslst,2,bcelst);
		UNPROTECT(4);
		return(anslst);
	}
}


#include "software/new_geom/polynomial1d.h"

#include <Eigen/Dense>
#include <Eigen/QR>
#include <list>
#include <stdexcept>

#include "software/new_geom/geom_constants.h"

Polynomial1d::Polynomial1d() {}

Polynomial1d::Polynomial1d(const std::vector<double> &coeffs) : coeffs(coeffs) {}

Polynomial1d::Polynomial1d(const std::initializer_list<double> &coeffs)
    : coeffs(std::vector<double>(coeffs))
{
}

Polynomial1d::Polynomial1d(const std::vector<Polynomial1d::Constraint> constraints)
{
    // Check that we have at least two constraints
    if (constraints.size() < 2)
    {
        throw std::invalid_argument(
            "Less then two constraints given, so no unique polynomial solution.");
    }

    // Check that all inputs are unique
    for (size_t i = 0; i < constraints.size(); i++)
    {
        for (size_t j = i + 1; j < constraints.size(); j++)
        {
            if (constraints[i].input == constraints[j].input)
            {
                throw std::invalid_argument(
                    "At least two inputs were equal, does not define a valid set of constraints");
            }
        }
    }

    // Solve for the coefficients
    Eigen::MatrixXd A(constraints.size(), constraints.size());
    Eigen::VectorXd b(constraints.size());

    for (size_t row_index = 0; row_index < constraints.size(); row_index++)
    {
        for (size_t col_index = 0; col_index < constraints.size(); col_index++)
        {
            A(row_index, col_index) =
                std::pow(constraints[row_index].input, static_cast<double>(col_index));
            b(row_index) = constraints[row_index].output;
        }
    }

    const Eigen::VectorXd coeff_vector = A.fullPivLu().solve(b);

    for (size_t i = 0; i < constraints.size(); i++)
    {
        coeffs.emplace_back(coeff_vector(i));
    }
}

double Polynomial1d::getCoeff(unsigned int order) const
{
    if (order >= coeffs.size())
    {
        return 0;
    }
    else
    {
        return coeffs[order];
    }
}

void Polynomial1d::setCoeff(unsigned int order, double coeff)
{
    if (order >= coeffs.size())
    {
        coeffs.resize(order + 1, 0);
    }
    coeffs[order] = coeff;
}

unsigned int Polynomial1d::getOrder() const
{
    if (coeffs.size() != 0)
    {
        for (unsigned int i = coeffs.size(); i > 0; i--)
        {
            if (std::abs(coeffs[i - 1]) >= GeomConstants::FIXED_EPSILON)
            {
                return i - 1;
            }
        }
    }
    // Zero polynomial treated as an order zero polynomial
    return 0;
}

double Polynomial1d::valueAt(double val) const
{
    // Horner's Method:
    // https://www.geeksforgeeks.org/horners-method-polynomial-evaluation/
    unsigned int order = getOrder();
    double retval      = getCoeff(order);
    for (unsigned int i = 1; i <= order; i++)
    {
        retval = retval * val + getCoeff(order - i);
    }
    return retval;
}

Polynomial1d operator+(const Polynomial1d &p1, const Polynomial1d &p2)
{
    Polynomial1d sum;
    unsigned int max_order = std::max(p1.getOrder(), p2.getOrder());
    for (unsigned int i = 0; i <= max_order; i++)
    {
        sum.setCoeff(i, p1.getCoeff(i) + p2.getCoeff(i));
    }
    return sum;
}

Polynomial1d operator-(const Polynomial1d &p1, const Polynomial1d &p2)
{
    Polynomial1d difference;
    unsigned int max_order = std::max(p1.getOrder(), p2.getOrder());
    for (unsigned int i = 0; i <= max_order; i++)
    {
        difference.setCoeff(i, p1.getCoeff(i) - p2.getCoeff(i));
    }
    return difference;
}

Polynomial1d operator*(const Polynomial1d &p1, const Polynomial1d &p2)
{
    Polynomial1d product;
    unsigned int p1_order = p1.getOrder();
    unsigned int p2_order = p2.getOrder();
    for (unsigned int i = 0; i <= p1_order; i++)
    {
        for (unsigned int j = 0; j <= p2_order; j++)
        {
            product.setCoeff(i + j,
                             product.getCoeff(i + j) + (p1.getCoeff(i) * p2.getCoeff(j)));
        }
    }
    return product;
}

Polynomial1d &operator+=(Polynomial1d &p1, const Polynomial1d &p2)
{
    return p1 = p1 + p2;
}

Polynomial1d &operator-=(Polynomial1d &p1, const Polynomial1d &p2)
{
    return p1 = p1 - p2;
}

Polynomial1d &operator*=(Polynomial1d &p1, const Polynomial1d &p2)
{
    return p1 = p1 * p2;
}

bool operator==(const Polynomial1d &p1, const Polynomial1d &p2)
{
    unsigned int p1_order = p1.getOrder();
    unsigned int p2_order = p2.getOrder();
    if (p1_order != p2_order)
    {
        return false;
    }
    for (unsigned int i = 0; i < p1_order; i++)
    {
        if (std::abs(p1.getCoeff(i) - p2.getCoeff(i)) >= GeomConstants::FIXED_EPSILON)
        {
            return false;
        }
    }
    return true;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/detail/diff_div.hpp>
#include <eve/function/diff/legendre.hpp>
#include <type_traits>
#include <boost/math/special_functions/legendre.hpp>

TTS_CASE_TPL("Check diff(legendre) return type", EVE_TYPE)
{
    TTS_EXPR_IS(eve::diff(eve::legendre)((unsigned int)(0), T()), T);
}

TTS_CASE_TPL("Check eve::diff(eve::legendre) behavior", EVE_TYPE)
{
  if constexpr(eve::floating_value<T>)
  {
    using i_t = eve::as_integer_t<T,unsigned>;
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(2u, T{0.5}), T(boost::math::legendre_p_prime(2u, 0.5)), 2.0);
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(2u, T{0.1}), T(boost::math::legendre_p_prime(2u, 0.1)), 2.0);
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(3u, T{0.5}), T(boost::math::legendre_p_prime(3u, 0.5)), 2.0);
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(3u, T{0.1}), T(boost::math::legendre_p_prime(3u, 0.1)), 2.0);

    TTS_ULP_EQUAL(eve::diff(eve::legendre)(i_t(2u), T{0.5}), T(boost::math::legendre_p_prime(2u, 0.5)), 2.0);
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(i_t(2u), T{0.1}), T(boost::math::legendre_p_prime(2u, 0.1)), 2.0);
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(i_t(3u), T{0.5}), T(boost::math::legendre_p_prime(3u, 0.5)), 2.0);
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(i_t(3u), T{0.1}), T(boost::math::legendre_p_prime(3u, 0.1)), 2.0);
    TTS_ULP_EQUAL(eve::diff(eve::legendre)(i_t(5u), T{0.1}), T(boost::math::legendre_p_prime(5u, 0.1)), 2.0);
  }
}


//  Boost common_factor_ct.hpp header file  ----------------------------------//

//  (C) Copyright Daryle Walker and Stephen Cleary 2001-2002.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_COMMON_FACTOR_CT_HPP
#define BOOST_MATH_COMMON_FACTOR_CT_HPP

#include <boost/math_fwd.hpp>  // self include
#include <boost/config.hpp>  // for BOOST_STATIC_CONSTANT, etc.
#include <boost/mpl/integral_c.hpp>

namespace boost
{
namespace math
{

//  Implementation details  --------------------------------------------------//

namespace detail
{
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build GCD with Euclid's recursive algorithm
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_gcd_helper_t
    {
    private:
        BOOST_STATIC_CONSTANT( static_gcd_type, new_value1 = Value2 );
        BOOST_STATIC_CONSTANT( static_gcd_type, new_value2 = Value1 % Value2 );

        #ifndef __BORLANDC__
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value) static_cast<static_gcd_type>(Value)
        #else
        typedef static_gcd_helper_t  self_type;
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value)  (self_type:: Value )
        #endif

        typedef static_gcd_helper_t< BOOST_DETAIL_GCD_HELPER_VAL(new_value1),
         BOOST_DETAIL_GCD_HELPER_VAL(new_value2) >  next_step_type;

        #undef BOOST_DETAIL_GCD_HELPER_VAL

    public:
        BOOST_STATIC_CONSTANT( static_gcd_type, value = next_step_type::value );
    };

    // Non-recursive case
    template < static_gcd_type Value1 >
    struct static_gcd_helper_t< Value1, 0UL >
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1 );
    };
#else
    // Use inner class template workaround from Peter Dimov
    template < static_gcd_type Value1 >
    struct static_gcd_helper2_t
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value
             = static_gcd_helper2_t<Value2>::BOOST_NESTED_TEMPLATE
             helper<Value1 % Value2>::value );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1 );
        };
    };

    // Special case
    template <  >
    struct static_gcd_helper2_t< 0UL >
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = Value2 );
        };
    };

    // Build the GCD from the above template(s)
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_gcd_helper_t
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value
         = static_gcd_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build the LCM from the GCD
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_lcm_helper_t
    {
        typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

        BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1 / gcd_type::value
         * Value2 );
    };

    // Special case for zero-GCD values
    template < >
    struct static_lcm_helper_t< 0UL, 0UL >
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value = 0UL );
    };
#else
    // Adapt GCD's inner class template workaround for LCM
    template < static_gcd_type Value1 >
    struct static_lcm_helper2_t
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

            BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1
             / gcd_type::value * Value2 );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = 0UL );
        };
    };

    // Special case
    template <  >
    struct static_lcm_helper2_t< 0UL >
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = 0UL );
        };
    };

    // Build the LCM from the above template(s)
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_lcm_helper_t
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value
         = static_lcm_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

}  // namespace detail

//  Compile-time greatest common divisor evaluator class declaration  --------//

template < static_gcd_type Value1, static_gcd_type Value2 >
struct static_gcd : public mpl::integral_c<static_gcd_type, (detail::static_gcd_helper_t<Value1, Value2>::value) >
{
};  // boost::math::static_gcd

//  Compile-time least common multiple evaluator class declaration  ----------//

template < static_gcd_type Value1, static_gcd_type Value2 >
struct static_lcm : public mpl::integral_c<static_gcd_type, (detail::static_lcm_helper_t<Value1, Value2>::value) >
{
};  // boost::math::static_lcm

}  // namespace math
}  // namespace boost

#endif  // BOOST_MATH_COMMON_FACTOR_CT_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>

template <typename P, typename T>
void test_area_circle()
{
    bg::model::nsphere<P, T> c;

    bg::set<0>(c, 0);
    bg::set<1>(c, 0);
    bg::set_radius<0>(c, 2);

    double d = bg::area(c);
    BOOST_CHECK_CLOSE(d, 4 * 3.1415926535897932384626433832795, 0.001);
}

int test_main(int, char* [])
{
    test_area_circle<bg::model::point<double, 2, bg::cs::cartesian>, double>();
    return 0;
}


/*
Copyright (c) 2016 Bastien Durix

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 DistanceField.cpp
 *  \brief Defines functions related to distance field
 *  \author Bastien Durix
 */

#include "DistanceField.h"
#include <boost/math/distributions/normal.hpp>
#include <set>
#include <nlopt.hpp>
#include <iostream>

unsigned int algorithm::skeletonization::closestInd(const boundary::DiscreteBoundary<2>::Ptr disbnd, const Eigen::Vector2d &C)
{
	double distmin = 0.0;
	unsigned int ind = 0;
	for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double dist = vec.norm();

		if(dist < distmin || i == 0)
		{
			distmin = dist;
			ind = i;
		}
	}

	return ind;
}

double algorithm::skeletonization::fieldValue(const boundary::DiscreteBoundary<2>::Ptr disbnd, const Eigen::Vector2d &C, double noise)
{
	boost::math::normal norm(0.0,noise);

	std::vector<double> dists(disbnd->getNbVertices());

	double distmin = 0.0;
	for(unsigned int i = 0; i < dists.size(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double dist = vec.norm();

		if(dist < distmin || i == 0) distmin = dist;

		dists[i] = dist;
	}

	double denom = 0.0;
	double numer = 0.0;

	for(unsigned int i = 0; i < dists.size(); i++)
	{
		double diff = dists[i] - distmin;
		double proba = 2.0*(1.0-boost::math::cdf(norm,diff));

		numer += proba * dists[i];

		denom += proba;
	}

	double val = numer/denom;

	if(val - distmin > 2.0)
	std::cout << val - distmin << std::endl;

	return val;
}

double algorithm::skeletonization::fieldValue(const boundary::DiscreteBoundary<2>::Ptr disbnd, const Eigen::Vector2d &C, Eigen::Vector2d &grad, double noise)
{
	boost::math::normal norm(0.0,noise);

	std::vector<double> dists(disbnd->getNbVertices());

	double distmin = 0.0;
	for(unsigned int i = 0; i < dists.size(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double dist = vec.norm();

		if(dist < distmin || i == 0) distmin = dist;

		dists[i] = dist;
	}

	double denom = 0.0;
	double numer = 0.0;
	Eigen::Vector2d numvec(0.0,0.0);

	for(unsigned int i = 0; i < dists.size(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double diff = dists[i] - distmin;
		double proba = 2.0*(1.0-boost::math::cdf(norm,diff));

		numer += proba * dists[i];

		numvec += proba*vec.normalized();

		denom += proba;
	}

	double val = numer/denom;

	if(val - distmin > 2.0)
	std::cout << val - distmin << std::endl;

	grad = numvec * (1.0/denom);

	return val;
}

void algorithm::skeletonization::tangencyBoundary(const boundary::DiscreteBoundary<2>::Ptr disbnd,
												  const Eigen::Vector2d &C,
												  const double &rad,
												  double noise,
												  std::vector<std::list<unsigned int> > &v_ind,
												  double fac)
{
	std::vector<bool> used(disbnd->getNbVertices(),false);
	std::vector<double> val(disbnd->getNbVertices());
	std::vector<bool> isin(disbnd->getNbVertices(),false);
	for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
	{
		val[i] = (disbnd->getVertex(i) - C).norm() - rad;
		if(val[i] < noise) isin[i] = true;
	}

	bool nochange = false;

	do
	{
		nochange = true;
		for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
		{
			if(!isin[i])
			{
				unsigned int prev = disbnd->getPrev(i);
				unsigned int next = disbnd->getNext(i);
				if((isin[prev] || isin[next]) && val[i] < fac)
				{
					isin[i] = true;
					nochange = false;
				}
			}
		}
	}while(!nochange);

	std::list<std::list<unsigned int> > l_ind;
	for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
	{
		if(isin[i] && !used[i])
		{
			std::list<unsigned int> cind;

			used[i] = true;
			unsigned int indn = i;
			do
			{
				cind.push_back(indn);
				used[indn] = true;
				indn = disbnd->getNext(indn);
			}while(isin[indn] && indn != *(cind.begin()));

			unsigned int indp = i;
			cind.pop_front();
			do
			{
				cind.push_front(indp);
				used[indp] = true;
				indp = disbnd->getPrev(indp);
			}while(isin[indp] && indp != *(cind.rbegin()));
			if(indp == *(cind.rbegin()))
				cind.push_front(indp);

			while(val[*(cind.begin())] > noise)
				cind.pop_front();
			while(val[*(cind.rbegin())] > noise)
				cind.pop_back();
			if(cind.size() > 1)
				l_ind.push_back(cind);
		}
	}

	l_ind.sort(
		[disbnd,C](const std::list<unsigned int> &c1, const std::list<unsigned int> &c2)
		{
			Eigen::Vector2d p1 = disbnd->getVertex(*(c1.begin())) - C;
			Eigen::Vector2d p2 = disbnd->getVertex(*(c2.begin())) - C;
			double ang1 = atan2(p1.y(),p1.x());
			double ang2 = atan2(p2.y(),p2.x());
			return ang1 < ang2;
		});
	v_ind = std::vector<std::list<unsigned int> >(l_ind.begin(),l_ind.end());
}

struct DataClosestCenter2d
{
	const boundary::DiscreteBoundary<2>::Ptr &disbnd;
	double noise;

	DataClosestCenter2d(
		const boundary::DiscreteBoundary<2>::Ptr &disbnd_,
		double noise_) :
		disbnd(disbnd_), noise(noise_) {};
};

double minClosestCenter2d(const std::vector<double> &vt, std::vector<double> &, void *dataFun)
{
	DataClosestCenter2d *data = (DataClosestCenter2d*) dataFun;

	Eigen::Vector2d C(vt[0],vt[1]);

	Eigen::Vector2d grad;
	algorithm::skeletonization::fieldValue(data->disbnd, C, grad, data->noise);
	//return -algorithm::skeletonization::recFieldValue(data->vdisbnd, data->vdisshp, data->vcam, C, data->noise);
	return grad.squaredNorm();
}

bool algorithm::skeletonization::closestCenter(const boundary::DiscreteBoundary<2>::Ptr disbnd, Eigen::Vector2d &C, double noise)
{
	unsigned int nbiter = 10000;

	bool fini = false;
	do
	{
		Eigen::Vector2d grad;
		fieldValue(disbnd,C,grad,noise);
		fini = grad.norm() < 0.01*noise;
		if(!fini) C -= grad * noise * 0.1;
		nbiter--;
	}while(nbiter != 0 && !fini);

	std::vector<double> lb(2);
	std::vector<double> ub(2);
	std::vector<double> C_init(2);

	Eigen::Vector2d grad;
	double rad = fieldValue(disbnd,C,grad,noise);

	C_init[0] = C.x();
	C_init[1] = C.y();

	lb[0] = C.x() - rad*0.2;
	lb[1] = C.y() - rad*0.2;

	ub[0] = C.x() + rad*0.2;
	ub[1] = C.y() + rad*0.2;

	DataClosestCenter2d data(disbnd,noise);

	nlopt::opt opt(nlopt::LN_COBYLA, 2);
	opt.set_lower_bounds(lb);
	opt.set_upper_bounds(ub);

	opt.set_min_objective(minClosestCenter2d, &data);

	opt.set_xtol_rel(1e-2);

	double res;
	opt.optimize(C_init, res);
	C.x() = C_init[0];
	C.y() = C_init[1];

	return true;
}

void algorithm::skeletonization::closestCenterOnArc(const boundary::DiscreteBoundary<2>::Ptr disbnd,
													const Eigen::Vector2d &C,
													const std::pair<double,double> &pang,
													double dist,
													Eigen::Vector2d &Cmov,
													double noise)
{
	double step = noise / (dist*dist);

	double ang1 = pang.first;
	double ang2 = pang.second;

	Eigen::Vector2d P1 = C + dist*Eigen::Vector2d(cos(ang1),sin(ang1));
	Eigen::Vector2d P2 = C + dist*Eigen::Vector2d(cos(ang2),sin(ang2));

	while(ang2 - ang1 > step)
	{
		double angmid = (ang1 + ang2) / 2.0;

		Eigen::Vector2d grad;
		Eigen::Vector2d vec(cos(angmid),sin(angmid));
		Cmov = C + dist * vec;
		fieldValue(disbnd,Cmov,grad,noise);

		//double der = dist*(grad.y() * vec.x() - grad.x() * vec.y());
		//if(der < 0.0) ang1 = angmid;
		//if(der > 0.0) ang2 = angmid;

		Eigen::Vector2d vec1 = (P1 - Cmov).normalized();
		Eigen::Vector2d vec2 = (P2 - Cmov).normalized();
		double sim1 = grad.dot(vec1);
		double sim2 = grad.dot(vec2);
		if(sim1 > sim2) ang1 = angmid;
		if(sim2 > sim1) ang2 = angmid;
	}

	double angmid = (ang1 + ang2) / 2.0;

	Eigen::Vector2d vec(cos(angmid),sin(angmid));
	Cmov = C + dist * vec;
}

void algorithm::skeletonization::discontinuitiesOnArc(const boundary::DiscreteBoundary<2>::Ptr disbnd,
													  const Eigen::Vector2d &C,
													  const std::pair<double,double> &pang,
													  double dist,
													  std::vector<Eigen::Vector2d> &vecC,
													  std::vector<std::pair<unsigned int,unsigned int> > &vecInd,
													  double noise)
{
	std::list<Eigen::Vector2d> lisC;
	std::list<std::pair<unsigned int,unsigned int> > lisInd;
	double step = noise / (dist*dist);

	double ang2 = pang.second;
	Eigen::Vector2d P2 = C + dist*Eigen::Vector2d(cos(ang2),sin(ang2));
	unsigned int ind2 = closestInd(disbnd,P2);

	double ang1 = pang.first;
	Eigen::Vector2d P1 = C + dist*Eigen::Vector2d(cos(ang1),sin(ang1));
	unsigned int ind1 = closestInd(disbnd,P1);

	//std::cout << "disc " << ind1 << " ";

	while(ind1 != ind2)
	{
		while(ang2 - ang1 > step)
		{
			double angmid = (ang1 + ang2) / 2.0;
			Eigen::Vector2d vec(cos(angmid),sin(angmid));
			Eigen::Vector2d Cmov = C + dist * vec;

			unsigned int indmid = closestInd(disbnd,Cmov);

			if(indmid == ind1)
			{
				ang1 = angmid;
			}
			else
			{
				ang2 = angmid;
			}
		}

		Eigen::Vector2d P1 = C + dist*Eigen::Vector2d(cos(ang1),sin(ang1));
		unsigned int ind1b = closestInd(disbnd,P1);
		Eigen::Vector2d P2 = C + dist*Eigen::Vector2d(cos(ang2),sin(ang2));
		unsigned int ind2b = closestInd(disbnd,P2);

		double angmid = (ang1 + ang2) / 2.0;
		Eigen::Vector2d vec(cos(angmid),sin(angmid));
		Eigen::Vector2d Cmov = C + dist * vec;

		lisC.push_back(Cmov);
		lisInd.push_back(std::make_pair(ind1b,ind2b));

		ind1 = ind2b;
		ang1 = ang2;
		ang2 = pang.second;
		//std::cout << ind1 << " ";
	}
	//std::cout << std::endl;

	vecC = std::vector<Eigen::Vector2d>(lisC.begin(),lisC.end());
	vecInd = std::vector<std::pair<unsigned int,unsigned int> >(lisInd.begin(),lisInd.end());
}


#include <vector>
#include <algorithm>
#include <cmath>
#include <glload/gl_all.hpp>
#include <glload/gl_load.hpp>

#include <boost/tuple/tuple.hpp>
#include "glmesh/BoostDraw.h"
#include "glmesh/CpuDataWriter.h"
#include "glmesh/VertexFormat.h"

#include "glmesh/Mesh.h"
#include "glmesh/GenQuadrics.h"
#include "GenHelper.h"
#include <glm/glm.hpp>

namespace glmesh
{
	namespace gen
	{
		namespace
		{
			const float g_pi = 3.1415726f;
			const float g_2pi = g_pi * 2.0f;
		}

		Mesh * UnitSphere( int numHorizSlices, int numVertSlices )
		{
			//The term "ring" refers to horizontal slices.
			//The term "segment" refers to vertical slices.

			//////////////////////////////////////////////////////////////////////////
			// Generate the vertex attribute data.
			numHorizSlices = std::max(numHorizSlices, 1);
			numVertSlices = std::max(numVertSlices, 3);

			//+2 to horizontal is for the top and bottom points, which are replicated due to texcoords.
			size_t numRingVerts = numHorizSlices + 2;
			//+1 to vertical is for doubling up on the initial point, again due to texcoords.
			size_t numSegVerts = numVertSlices + 1;
			size_t attribCount = numSegVerts * numRingVerts;

			glmesh::AttributeList attribs;
			attribs.push_back(glmesh::AttribDesc(ATTR_POS, 3, glmesh::VDT_SINGLE_FLOAT, glmesh::ADT_FLOAT));
			attribs.push_back(glmesh::AttribDesc(ATTR_NORMAL, 3, glmesh::VDT_SINGLE_FLOAT, glmesh::ADT_FLOAT));
			attribs.push_back(glmesh::AttribDesc(ATTR_TEXCOORD, 2, glmesh::VDT_SINGLE_FLOAT, glmesh::ADT_FLOAT));

			VertexFormat fmt(attribs);

			CpuDataWriter writer(fmt, attribCount);

			float deltaSegTexCoord = 1.0f / numSegVerts;
			float deltaRingTexCoord = 1.0f / numRingVerts;

			for(int segment = 0; segment < numVertSlices; ++segment)
			{
				writer.Attrib(0.0f, 1.0f, 0.0f);
				writer.Attrib(0.0f, 1.0f, 0.0f);
				writer.Attrib(deltaSegTexCoord * segment, 1.0f);
			}

			writer.Attrib(0.0f, 1.0f, 0.0f);
			writer.Attrib(0.0f, 1.0f, 0.0f);
			writer.Attrib(1.0f, 0.0f);

			float radThetaDelta = g_pi / (numHorizSlices + 1);
			float radRhoDelta = g_2pi / numVertSlices;

			for(int ring = 0; ring < numHorizSlices; ++ring)
			{
				float radTheta = radThetaDelta * (ring + 1);
				float sinTheta = std::sin(radTheta);
				float cosTheta = std::cos(radTheta);

				float ringTexCoord = 1.0f - ((ring + 1) * deltaRingTexCoord);

				for(int segment = 0; segment < numVertSlices; ++segment)
				{
					float radRho = radRhoDelta * segment;
					float sinRho = std::sin(-radRho);
					float cosRho = std::cos(-radRho);

					glm::vec3 currPos(sinTheta * cosRho, cosTheta, sinTheta * sinRho);
					writer.Attrib(currPos);
					writer.Attrib(currPos);
					writer.Attrib(deltaSegTexCoord * segment, ringTexCoord);
				}

				writer.Attrib(sinTheta, cosTheta, 0.0f);
				writer.Attrib(sinTheta, cosTheta, 0.0f);
				writer.Attrib(1.0f, ringTexCoord);
			}

			for(int segment = 0; segment < numVertSlices; ++segment)
			{
				writer.Attrib(0.0f, -1.0f, 0.0f);
				writer.Attrib(0.0f, -1.0f, 0.0f);
				writer.Attrib(deltaSegTexCoord * segment, 0.0f);
			}

			writer.Attrib(0.0f, 1.0f, 0.0f);
			writer.Attrib(0.0f, 1.0f, 0.0f);
			writer.Attrib(1.0f, 0.0f);

			//////////////////////////////////////////////////////////////////////////
			//Generate the index data.
			//Restart index.
			GLuint restartIndex = writer.GetNumVerticesWritten();

			size_t stripSize = ((2 * numVertSlices) + 2);
			//One strip for each ring vertex list, minus 1.
			size_t numStrips = (numRingVerts - 1);

			size_t numIndices = numStrips * stripSize;
			//Add one index between each strip for primitive restarting.
			numIndices += (numStrips - 1);

			std::vector<GLuint> indices;
			indices.reserve(numIndices);

			for(size_t strip = 0; strip < numStrips; ++strip)
			{
				GLuint topRingIndex = (strip * numSegVerts);
				GLuint botRingIndex = ((strip + 1) * numSegVerts);

				for(size_t segment = 0; segment < numSegVerts; ++segment)
				{
					indices.push_back(topRingIndex + segment);
					indices.push_back(botRingIndex + segment);
				}

				if(indices.size() != numIndices)
					indices.push_back(restartIndex);
			}

			//////////////////////////////////////////////////////////////////////////
			//Build the buffers.
			std::vector<GLuint> buffers(2);

			gl::GenBuffers(2, &buffers[0]);
			writer.TransferToBuffer(gl::ARRAY_BUFFER, gl::STATIC_DRAW, buffers[0]);

			//vertex data done. Now build the index buffer.
			gl::BindBuffer(gl::ELEMENT_ARRAY_BUFFER, buffers[1]);
			gl::BufferData(gl::ELEMENT_ARRAY_BUFFER, indices.size() * sizeof(GLuint),
				&indices[0], gl::STATIC_DRAW);
			gl::BindBuffer(gl::ELEMENT_ARRAY_BUFFER, 0);

			//Create VAOs.
			MeshVariantMap variantMap;

			gl::BindBuffer(gl::ARRAY_BUFFER, buffers[0]);

			GLuint currVao = 0;

			gl::GenVertexArrays(1, &currVao);
			gl::BindVertexArray(currVao);
			gl::BindBuffer(gl::ELEMENT_ARRAY_BUFFER, buffers[1]);
			fmt.BindAttribute(0, 0);
			AddVariantToMap(variantMap, currVao, 0);

			gl::GenVertexArrays(1, &currVao);
			gl::BindVertexArray(currVao);
			gl::BindBuffer(gl::ELEMENT_ARRAY_BUFFER, buffers[1]);
			fmt.BindAttribute(0, 0);
			fmt.BindAttribute(0, 1);
			AddVariantToMap(variantMap, currVao, VAR_NORMAL);

			gl::GenVertexArrays(1, &currVao);
			gl::BindVertexArray(currVao);
			gl::BindBuffer(gl::ELEMENT_ARRAY_BUFFER, buffers[1]);
			fmt.BindAttribute(0, 0);
			fmt.BindAttribute(0, 2);
			AddVariantToMap(variantMap, currVao, VAR_TEX_COORD);

			gl::GenVertexArrays(1, &currVao);
			gl::BindVertexArray(currVao);
			gl::BindBuffer(gl::ELEMENT_ARRAY_BUFFER, buffers[1]);
			fmt.BindAttribute(0, 0);
			fmt.BindAttribute(0, 1);
			fmt.BindAttribute(0, 2);
			AddVariantToMap(variantMap, currVao, VAR_TEX_COORD | VAR_NORMAL);

			gl::BindVertexArray(0);
			gl::BindBuffer(gl::ARRAY_BUFFER, 0);

			//////////////////////////////////////////////////////////////////////////
			//Create rendering commands.
			RenderCmdList renderCmds;
			if(glload::IsVersionGEQ(3, 1))
			{
				//Has primitive restart. Therefore, can draw two fans as one.
				renderCmds.PrimitiveRestartIndex(restartIndex);
				renderCmds.DrawElements(gl::TRIANGLE_STRIP, numIndices, gl::UNSIGNED_INT, 0);
				renderCmds.PrimitiveRestartIndex();
			}
			else
			{
				//No restart. Must draw each strip one after the other.
				for(size_t strip = 0; strip < numStrips; ++strip)
				{
					GLuint stripStart = strip * (stripSize + 1);

					renderCmds.DrawElements(gl::TRIANGLE_STRIP, stripSize, gl::UNSIGNED_INT,
						stripStart * sizeof(GLuint));
				}
			}

			GLuint mainVao = variantMap["lit-tex"];

			Mesh *pRet = new Mesh(buffers, mainVao, renderCmds, variantMap);
			return pRet;
		}
	}
}


// minphase.cc
//
// [y,ssp,iter] = minphase(h)
// C++ implementation of minphase.m with the Eigen C++ template library:
//   m-file for extracting the minimum phase factor from the
//   linear-phase filter h. Input: h = (h(0) h(1)...h(N)] (row vector)
//   where the h vector is the right half of a linear-phase FIR filter.
//   It is presumed that any unit-circle zeros of h are of even multiplicity.
//   Copyright (c) January 2002  by  H. J. Orchard and A. N. Willson, Jr.

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

#include <cfloat>
#include <Eigen/Eigen>
#include <octave/oct.h>
#include <octave/parse.h>

typedef Eigen::Matrix<long double,Eigen::Dynamic,1> EigenVectorNx1ld;
typedef Eigen::Matrix<long double,Eigen::Dynamic,Eigen::Dynamic>
  EigenMatrixNxNld;

DEFUN_DLD(minphase, args, nargout,"[y,ssp,iter] = minphase(h)")
{

  // Sanity checks
  octave_idx_type nargin=args.length();
  if ((nargin>1) || (nargout>3))
    {
      print_usage();
    }

  // Input arguments
  ColumnVector h = args(0).column_vector_value();
  octave_idx_type N=h.numel();

  // Output arguments
  RowVector y(N);
  long double ssp = LDBL_MAX;
  octave_idx_type iter = 0;

  // Initialise ss
  long double ss=ssp/2;

  // Initialise column vector hh
  EigenVectorNx1ld hh(N);
  for(auto r=0;r<N;r++)
    {
      hh(r)=h(r);
    }

  // Initialise column vectors yy, b and d
  EigenVectorNx1ld yy(N);
  EigenVectorNx1ld d(N);
  EigenVectorNx1ld b(N);
  for (auto r=0;r<N;r++)
    {
      yy(r)=0;
      d(r)=0;
    }
  yy(0)=1;

  // Allocate A, Al and Ar
  EigenMatrixNxNld A(N,N);
  EigenMatrixNxNld Al(N,N);
  EigenMatrixNxNld Ar(N,N);

  for (auto r=0;r<N;r++)
    {
      for (auto c=0;c<N;c++)
        {
          A(r,c)=0;
          Al(r,c)=0;
          Ar(r,c)=0;
        }
    }

  // Newton-Raphson iteration
  while (ss < ssp)
    {
      yy=yy+d;
      ssp=ss;
      iter=iter+1;

      for(auto r=0;r<N;r++)
        {
          for (auto c=r;c<N;c++)
            {
              Al(r,c-r)=yy(c);
            }
          for (auto c=0;c<(N-r);c++)
            {
              Ar(r,c+r)=yy(c);
            }
        }
      A=Al+Ar;
      b=hh-(Al*yy);
      d=A.colPivHouseholderQr().solve(b);
      ss=d.norm();
    }

  // Done
  for(auto r=0;r<N;r++)
    {
      y(r)=(double)(yy(r));
    }
  octave_value_list retval(nargout);
  if (nargout >= 1)
    {
      retval(0)=y;
    }
  if (nargout >= 2)
    {
      retval(1)=(double)ssp;
    }
  if (nargout == 3)
    {
      retval(2)=iter;
    }
  return retval;
}


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

#include "../include/KF.h"
#include "../include/dsho.h"
#include "../include/matern32.h"
#include "../include/ndsho.h"

using namespace Eigen;
using namespace std;

// function to read a space separated file

std::vector<double> load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ' ')) {
            //cout << std::stod(cell) << endl;
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return values;
}

int main()
{

std::random_device rd;
std::mt19937 rng(rd());

// read file with two component damped simple harmonic oscillator simulated data
//std::vector<double> values = load_csv("two_comp_dsho.txt");
//cout << values.size() << endl;

// map data to VectorXds
//VectorXd times = Map<VectorXd, 0, InnerStride<2> > (values.data(), 10000);
//VectorXd yi = Map<VectorXd, 0, InnerStride<2> > (values.data()+1, 10000);
int vecsize = 10000;
VectorXd yi(vecsize);
VectorXd times = Eigen::VectorXd::Random(vecsize);
times.array() += 1.0;
times.array() *= (100*0.5);
std::sort(times.data(), times.data() + times.size());

// this is the observational error vector
VectorXd yerr = VectorXd::Ones(yi.size());

double obs_err = 0.05;
yerr.array() *= obs_err;

std::uniform_real_distribution<double> uniform(0, 5);

// Now we run a bunch of tests

// create a Matern 3/2 solver and get likelihood of data
gpstate::matern32::Matern32Solver m32(times, yi, yerr,std::sqrt(10), 1.0);
double log_likelihood = m32.KF_log_likelihood();
cout << "logL: " << log_likelihood << endl;

// simulate a Matern 3/2 on the times given by VectorXd times
VectorXd y_sim;
m32.simulate_Matern32(y_sim, rng);

// set parameters for a 2 component DSHO model
Eigen::Vector2d omegas,Qpars,varfs;
omegas << 1, 3*0.15915494309;
Qpars << 10, 10;
varfs << 1, 1;

// set parameters for a single DSHO model
double omega0=12;
double Q=1;
double varf=1;

// simulate a single DSHO with parameters omega0, Q, varf
// we simulate for various values of Q, and measure the simulated variance
// objective is to verify that the variance specified by 'varf'
// corresponds indeed to the actual simulated variance
// (in other words, we are verifying the normalizations of our GPs)
gpstate::dsho::DSHOSolver dsho(times,yi,yerr,omega0, Q, varf);
dsho.simulate_DSHO(y_sim);

double mean = y_sim.mean();
Eigen::VectorXd tmp = y_sim.array()-mean;
double variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var1:" << variance << std::endl;

Q = 10;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var2:" << variance << std::endl;

Q = 0.1;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var3:" << variance << std::endl;

Q=10;
omega0=6;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var4:" << variance << std::endl;

omega0=120;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var5:" << variance << std::endl;
log_likelihood = dsho.KF_log_likelihood();
cout << "logL: " << log_likelihood << endl;

//std::exit(0);

Eigen::Matrix<double,1,1> omega_one, Qpar_one, varf_one;
omega_one << 120;
Qpar_one << 10;
varf_one << 1;

// Verify that the ndsho implementation is consistent with the dsho one
gpstate::n_dsho::N_DSHOSolver ndsho(times,yi,yerr,omega_one, Qpar_one, varf_one);
ndsho.simulate_N_DSHO(y_sim, rng);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var6:" << variance << std::endl;
log_likelihood = ndsho.KF_log_likelihood();
cout << "logL: " << log_likelihood << endl;

// write simulated vector to file GPtest.txt
std::ofstream file("GPtest.txt");
if (file.is_open())
  {
    file << "#time y_sim" << endl;
    for(int i=0; i<times.rows(); i++)
    {
        file <<  times(i) <<" "<< y_sim(i) << endl;
    }
  }

}


#pragma once

#include <Geometry/LineSegment.hpp>

#include <opencv2/core/core.hpp>
#include <Eigen/Dense>

#include <numeric>
#include <vector>

namespace pcv
{

/**
 * Utility function to help determine "leftness" of a point compared to the line
 * segment drawn between the first two points. Leftness is indicated by a
 * negative return value, which is why the tempalate parameter must be signed.
 * Idea from Computational Geometry in C by Joseph O'Rourke.
 */
template<class SignedNumericType>
SignedNumericType getTwiceSignedArea2D(
    const Eigen::Matrix<SignedNumericType, 2, 1> &endpoint0,
    const Eigen::Matrix<SignedNumericType, 2, 1> &endpoint1,
    const Eigen::Matrix<SignedNumericType, 2, 1> &testpoint)
{
    static_assert(std::is_signed<SignedNumericType>::value);

    // Aliases for more readable arithmetic.
    const auto &a = endpoint0;
    const auto &b = endpoint1;
    const auto &c = testpoint;

    return ((b.x() - a.x()) * (c.y() - a.y()))
         - ((c.x() - a.x()) * (b.y() - a.y()));
}

/**
 * \class ConvexPolygon
 * Create a polygon by specifying its vertices in a clockwise direction. Points
 * are 2D Eigen::Vectors.
 */
template<class NumericType>
class ConvexPolygon
{
public:
    ConvexPolygon() = default;

    ConvexPolygon(const ConvexPolygon &polygon) = default;

    /**
     * Move construction leverages std::vector's move constructor.
     */
    ConvexPolygon(ConvexPolygon &&polygon) = default;

    ~ConvexPolygon() = default;

    ConvexPolygon(const std::vector<Eigen::Matrix<NumericType, 2, 1>> &vertices) : vertices(vertices)
    {}

    ConvexPolygon(std::vector<Eigen::Matrix<NumericType, 2, 1>> &&vertices) : vertices(vertices)
    {}

    void addVertex(Eigen::Matrix<NumericType, 2, 1> &&vertex)
    {
        vertices.emplace_back(vertex);
    }

    void addVertex(const Eigen::Matrix<NumericType, 2, 1> &vertex)
    {
        vertices.push_back(vertex);
    }

    std::vector<Eigen::Matrix<NumericType, 2, 1>> getVertices() const
    {
        return vertices;
    }

    /**
     * Determine if a given point is inside or outside the polygon in O(n) where
     * n is the number of vertices of the polygon. This function requires a
     * point to be represented as two doubles, even if the points can be
     * represented with integers or the polygon has a non-floating point
     * templated NumericType. The current implementation does not account for
     * floating-point error.
     */
    bool isPointContained(Eigen::Vector2d testPoint) const;

    /**
     * Constructs a ConvexPolygon representing the minimum size rectangle with
     * sides perpendicular and parallel to the x-axis that completely encompasses
     * all points in the polygon.
     */
    ConvexPolygon<NumericType> getBoundingBox() const;

    /**
     * Transforms the polygon in-place with a 3x3 homographic transformation
     * matrix. Can only transform floating-point vertex definitions of polygons.
     * This method is not thread-safe.
     */
    void transform(const Eigen::Matrix3d &transform);

    /**
     * Get the ConvexPolygon that bounds the intersection of the polygons. The
     * public interface makes no runtime performance guarantee!
     */
    ConvexPolygon<double> getIntersectionWith(
            const ConvexPolygon<NumericType> &rhsPoly) const;

private:
    std::vector<Eigen::Matrix<NumericType, 2, 1>> vertices;
};

/**
 * Need to use another template parameter for rhsPoly's numeric type.
 */
template<class NumericType>
ConvexPolygon<double> ConvexPolygon<NumericType>::getIntersectionWith(
        const ConvexPolygon<NumericType> &rhsPoly) const
{
    const auto rhsVertices = rhsPoly.getVertices();
    auto rhsPrevVertex = rhsVertices.back();
    auto lhsPrevVertex = vertices.back();

    ConvexPolygon<double> intersection(
        std::accumulate(
            std::cbegin(vertices),
            std::cend(vertices),
            std::vector<Eigen::Vector2d>{},
            [&lhsPrevVertex, &rhsPrevVertex, &rhsVertices](
                std::vector<Eigen::Vector2d> accumulator,
                decltype(lhsPrevVertex) lhsVertexIter)
            {
                for (const auto &rhsVertexIter : rhsVertices)
                {
                    auto intersection = getSegmentIntersectionPoint2(
                        lhsPrevVertex, lhsVertexIter, rhsPrevVertex, rhsVertexIter);
                    if (intersection)
                    {
                        accumulator.emplace_back(*intersection);
                    }
                    rhsPrevVertex = rhsVertexIter;
                }

                lhsPrevVertex = lhsVertexIter;
                return accumulator;
            }));

    return intersection;
}

template<class NumericType>
bool ConvexPolygon<NumericType>::isPointContained(
    Eigen::Vector2d testPoint) const
{
    auto cachedVertex = vertices.back();

    return std::accumulate(
        std::cbegin(vertices),
        std::cend(vertices),
        true,
        [&cachedVertex, testPoint](bool isContained, decltype(cachedVertex) vertexIter)
        {
            isContained &=
                getTwiceSignedArea2D(
                    static_cast<Eigen::Vector2d>(cachedVertex.template cast<double>()),
                    static_cast<Eigen::Vector2d>(vertexIter.template cast<double>()),
                    testPoint) <= 0;

            cachedVertex = vertexIter;
            return isContained;
        });
}

template<class NumericType>
ConvexPolygon<NumericType> ConvexPolygon<NumericType>::getBoundingBox() const
{
    NumericType minX = vertices[0].x();
    NumericType minY = vertices[0].y();
    NumericType maxX = vertices[0].x();
    NumericType maxY = vertices[0].y();

    // Start iterator one after the beginning.
    const auto vertex = vertices.cbegin();
    std::advance(vertices, 1);
    for(; vertex != std::cend(vertex); std::next(vertex))
    {
        minX = std::min(vertex.x(), minX);
        minY = std::min(vertex.y(), minY);
        maxX = std::max(vertex.x(), maxX);
        maxY = std::max(vertex.y(), maxY);
    }

    // This is weird, the size is not dynamic, it's 2 because the points are 2D.
    ConvexPolygon<NumericType> output;
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        minX, minY});
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        minX, maxY});
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        maxX, maxY});
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        maxX, minY});

    return output;
}

/**
 * An O(n) operation where n is the max of the two polygon's number of vertices.
 */
template<class NumericType>
bool operator==(
    const ConvexPolygon<NumericType> &lhs,
    const ConvexPolygon<NumericType> &rhs)
{
    return lhs.getVertices() == rhs.getVertices();
}

template<class NumericType>
Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>
makePolygonIntersectionEigenGrid(
    const std::vector<ConvexPolygon<NumericType>> &polygons,
    std::size_t gridRows,
    std::size_t gridCols)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have numerical polygon type.");

    Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> mask =
        Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>::Zero(gridRows, gridCols);

    for (std::size_t r = 0; r < gridRows; ++r)
    {
        for (std::size_t c = 0; c < gridCols; ++c)
        {
            bool containment = true;
            for (const auto &polygon : polygons)
            {
                containment &= polygon.isPointContained(Eigen::Vector2d{c, gridRows-r-1});
            }
            mask(r, c) = containment;
        }
    }

    return mask;
}

template<class PolygonNumericType, class MatNumericType>
void makePolygonIntersectionOpencvGrid(
    const std::vector<ConvexPolygon<PolygonNumericType>> &polygons,
    std::size_t gridRows,
    std::size_t gridCols,
    cv::Mat_<MatNumericType> &output)
{
    cv::Mat_<MatNumericType> mask(gridRows, gridCols);

    for (std::size_t r = 0; r < gridRows; ++r)
    {
        for (std::size_t c = 0; c < gridCols; ++c)
        {
            bool containment = true;
            for (const auto &polygon : polygons)
            {
                containment &= polygon.isPointContained(Eigen::Vector2d{c, gridRows-r-1});
            }
            mask(r, c) = static_cast<MatNumericType>(containment);
        }
    }

    output = mask;
}

template<class NumericType>
inline void ConvexPolygon<NumericType>::transform(const Eigen::Matrix3d &transformMatrix)
{
    static_assert(std::is_floating_point<NumericType>::value,
                  "Can only perform in-place transformations with floating-point"
                  " polygons.");

    const auto &tform = transformMatrix;
    std::transform(
        std::begin(vertices), std::end(vertices), std::begin(vertices),
        [tform](decltype(vertices[0]) vertex)
        {
            Eigen::Matrix<NumericType, 3, 1> homogenousVertex;
            homogenousVertex << vertex[0], vertex[1], 1;

            Eigen::Matrix<NumericType, 3, 1> homogenousNewVertex =
                tform * homogenousVertex;

            Eigen::Matrix<NumericType, 2, 1> newVertex;
            newVertex << homogenousNewVertex[0], homogenousNewVertex[1];

            // Bring back to pixel coordinates.
            newVertex /= homogenousNewVertex[2];
            return newVertex;
        });
}

} // end namespace pcv


#define BOOST_NUMERIC_FUNCTIONAL_STD_COMPLEX_SUPPORT 1
#define BOOST_NUMERIC_FUNCTIONAL_STD_VALARRAY_SUPPORT 1
#define BOOST_NUMERIC_FUNCTIONAL_STD_VECTOR_SUPPORT 1
#include <complex>
#include <iostream>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics.hpp>

// this code comes from:
// http://hiankun.blogspot.de/2011/09/using-boost-accumulators-to-calculate.html
int main(){
  //using namespace boost::accumulators;
    boost::accumulators::accumulator_set< double, boost::accumulators::stats<boost::accumulators::tag::variance> > acc_variance;

    for (int i = 0; i < 10; i++){
        std::cout << i << ", ";
        acc_variance(i);
    }

    std::cout << std::endl << "Variance = "
              << sqrt(boost::accumulators::variance(acc_variance)) << std::endl;

    std::cout << std::endl << "Mean = "
        << boost::accumulators::mean(acc_variance) << std::endl;

    return 0;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

#ifndef SMOOTH__INTERNAL__C1_HPP_
#define SMOOTH__INTERNAL__C1_HPP_

#include <Eigen/Core>

#include "common.hpp"

namespace smooth {

/**
 * @brief C(1) Lie group of rotation and scaling
 *
 * Elements are on the form \f$ k R \f$ where \f$ k > 0 \f$ is a scalar and \f$ R \in SO(2) \f$.
 *
 * Memory layout
 * -------------
 * Group:    a b
 * Tangent:  s Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * [ b -a ]
 * [ a  b ]
 *
 * Lie algebra Matrix form
 * -----------------------
 * [ s -Ωz ]
 * [ Ωz  s ]
 *
 * Constraints
 * -----------
 * Group:   a * a - b * b > 0
 * Tangent: -pi < Ωz <= pi
 */
template<typename _Scalar>
class C1Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 2;
  static constexpr Eigen::Index Dim     = 2;
  static constexpr Eigen::Index Dof     = 2;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out << Scalar(0), Scalar(1); }

  static void setRandom(GRefOut g_out)
  {
    using std::sin, std::cos;

    const Scalar u = Eigen::internal::template random_impl<Scalar>::run(0, 2 * M_PI);
    const Scalar t = Eigen::internal::template random_impl<Scalar>::run(0.01, 100);
    g_out << t * sin(u), t * cos(u);
  }

  static void matrix(GRefIn g_in, MRefOut m_out) { m_out << g_in(1), -g_in(0), g_in(0), g_in(1); }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    g_out << g_in1[0] * g_in2[1] + g_in1[1] * g_in2[0], g_in1[1] * g_in2[1] - g_in1[0] * g_in2[0];
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    const Scalar t = g_in[0] * g_in[0] + g_in[1] * g_in[1];
    g_out << -g_in[0] / t, g_in[1] / t;
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using std::atan2, std::sqrt, std::log;

    const Scalar t = sqrt(g_in[0] * g_in[0] + g_in[1] * g_in[1]);
    a_out << log(t), atan2(g_in[0], g_in[1]);
  }

  static void Ad(GRefIn, TMapRefOut A_out) { A_out.setIdentity(); }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using std::cos, std::exp, std::sin;

    const Scalar t = exp(a_in.x());
    g_out << t * sin(a_in.y()), t * cos(a_in.y());
  }

  static void hat(TRefIn a_in, MRefOut A_out) { A_out << a_in(0), -a_in(1), a_in(1), a_in(0); }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    a_out << (A_in(0, 0) + A_in(1, 1)) / Scalar(2), (A_in(1, 0) - A_in(0, 1)) / Scalar(2);
  }

  static void ad(TRefIn, TMapRefOut A_out) { A_out.setZero(); }

  static void dr_exp(TRefIn, TMapRefOut A_out) { A_out.setIdentity(); }

  static void dr_expinv(TRefIn, TMapRefOut A_out) { A_out.setIdentity(); }
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__C1_HPP_


//
// Created by matt on 21/01/2021.
//

#include "simple_kf.h"

#include <iostream>

#include <Eigen/Core>
#include <Eigen/LU>

namespace kf0 {

KalmanFilter::KalmanFilter() {
  // State, position (x, y) and velocity (x, y)
  x_.setZero();
  // Initialise covariance with high values
  P_.setZero();
  P_.diagonal() = Eigen::Vector4d::Constant(100.0);
  // Process model
  // v' = v
  // p' = p + v*dt
  F_.setIdentity();
  F_(0, 2) = dt; // px' = px + vx*dt
  F_(1, 3) = dt; // py' = py + vy*dt
  // Process noise
  Q_ = Eigen::Matrix<double, 4, 4>::Identity() * 2.0; // TODO placeholder
  // Measurement model
  // Measure position
  H_.setZero();
  H_(0, 0) = 1.0;
  H_(1, 1) = 1.0;
  // Measurement noise
  R_.setIdentity(); // TODO placeholder

  //    std::cout << "F: " << '\n';
  //    std::cout << F_ << '\n';
  //    std::cout << "x: " << '\n';
  //    std::cout << x_.transpose() << '\n';
  //    std::cout << "P: " << '\n';
  //    std::cout << P_ << '\n';
  //    std::cout << "Q: " << '\n';
  //    std::cout << Q_ << '\n';
  //    std::cout << "H: " << '\n';
  //    std::cout << H_ << '\n';
  //    std::cout << "R: " << '\n';
  //    std::cout << R_ << "\n\n";
}

Eigen::Matrix<double, 4, 1> KalmanFilter::Predict() {
  x_ = F_ * x_;
  P_ = F_ * P_ * F_.transpose() + Q_;

  //    std::cout << "Predict\n";
  //    std::cout << "F: " << '\n';
  //    std::cout << F_ << '\n';
  //    std::cout << "x: " << '\n';
  //    std::cout << x_.transpose() << '\n';
  //    std::cout << "P: " << '\n';
  //    std::cout << P_ << "\n\n";
  return x_;
}

Eigen::Matrix<double, 4, 1>
KalmanFilter::Update(const Eigen::Matrix<double, 2, 1> &z) {
  z_hat_ = H_ * x_;
  y_ = z - z_hat_;
  S_ = H_ * P_ * H_.transpose() + R_;
  K_ = P_ * H_.transpose() * S_.inverse();
  x_ = x_ + K_ * y_;
  P_ = (Eigen::Matrix4d::Identity() - K_ * H_) * P_;

  //    std::cout << "Update\n";
  //    std::cout << "z: " << '\n';
  //    std::cout << z.transpose() << '\n';
  //    std::cout << "z_hat: " << '\n';
  //    std::cout << z_hat_.transpose() << '\n';
  //    std::cout << "y: " << '\n';
  //    std::cout << y_.transpose() << '\n';
  //    std::cout << "S: " << '\n';
  //    std::cout << S_ << '\n';
  //    std::cout << "K: " << '\n';
  //    std::cout << K_ << '\n';
  //    std::cout << "x: " << '\n';
  //    std::cout << x_.transpose() << '\n';
  //    std::cout << "P: " << '\n';
  //    std::cout << P_ << "\n\n";

  return x_;
}

} // namespace kf0


// 1-d PIC code to solve plasma two-stream instability problem.

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <blitz/array.h>
#include <fftw.h>

using namespace blitz;

void Output (char* fn1, char* fn2, double t,
	     Array<double,1> r, Array<double,1> v);
void Density (Array<double,1> r, Array<double,1>& n);
void Electric (Array<double,1> phi, Array<double,1>& E);
void Poisson1D (Array<double,1>& u, Array<double,1> v, double kappa);
void rk4_fixed (double& x, Array<double,1>& y,
              void (*rhs_eval)(double, Array<double,1>, Array<double,1>&),
              double h);
void rhs_eval (double t, Array<double,1> y, Array<double,1>& dydt);
void Load (Array<double,1> r, Array<double,1> v, Array<double,1>& y);
void UnLoad (Array<double,1> y, Array<double,1>& r, Array<double,1>& v);
double distribution (double vb);

double L; int N, J;

int main()
{
  // Parameters
  L;            // Domain of solution 0 <= x <= L (in Debye lengths)
  N;            // Number of electrons
  J;            // Number of grid points
  double vb;    // Beam velocity
  double dt;    // Time-step (in inverse plasma frequencies)
  double tmax;  // Simulation run from t = 0. to t = tmax

  // Get parameters
  printf ("Please input N:  "); scanf ("%d", &N);
  printf ("Please input vb:  "); scanf ("%lf", &vb);
  printf ("Please input L:  "); scanf ("%lf", &L);
  printf ("Please input J:  "); scanf ("%d", &J);
  printf ("Please input dt:  "); scanf ("%lf", &dt);
  printf ("Please input tmax:  "); scanf ("%lf", &tmax);
  int skip = int (tmax / dt) / 10;
  if ((N < 1) || (J < 2) || (L <= 0.) || (vb <= 0.)
      || (dt <= 0.) || (tmax <= 0.) || (skip < 1))
    {
      printf ("Error - invalid input parameters\n");
      exit (1);
    }

  // Set names of output files
  char* phase[11]; char* data[11];
  phase[0] = "phase0.out";phase[1] = "phase1.out";phase[2] = "phase2.out";
  phase[3] = "phase3.out";phase[4] = "phase4.out";phase[5] = "phase5.out";
  phase[6] = "phase6.out";phase[7] = "phase7.out";phase[8] = "phase8.out";
  phase[9] = "phase9.out";phase[10] = "phase10.out";data[0] = "data0.out";
  data[1] = "data1.out"; data[2] = "data2.out"; data[3] = "data3.out";
  data[4] = "data4.out"; data[5] = "data5.out"; data[6] = "data6.out";
  data[7] = "data7.out"; data[8] = "data8.out"; data[9] = "data9.out";
  data[10] = "data10.out";

  // Initialize solution
  double t = 0.;
  int seed = time (NULL); srand (seed);
  Array<double,1> r(N), v(N);
  for (int i = 0; i < N; i++)
    {
      r(i) = L * double (rand ()) / double (RAND_MAX);
      v(i) = distribution (vb);
    }
  Output (phase[0], data[0], t, r, v);

  // Evolve solution
  Array<double,1> y(2*N);
  Load (r, v, y);
  for (int k = 1; k <= 10; k++)
    {
      for (int kk = 0; kk < skip; kk++)
        {
           // Take time-step
           rk4_fixed (t, y, rhs_eval, dt);

           // Make sure all coordinates in range 0 to L.
           for (int i = 0; i < N; i++)
             {
               if (y(i) < 0.) y(i) += L;
               if (y(i) > L) y(i) -= L;
             }

           printf ("t = %11.4e\n", t);
        }
      printf ("Plot %3d\n", k);

      // Output data
      UnLoad (y, r, v);
      Output(phase[k], data[k], t, r, v);
    }

  return 0;
}
The following routine outputs the simulation data to various data-files.

// Write data to output files

void Output (char* fn1, char* fn2, double t,
	     Array<double,1> r, Array<double,1> v)
{
  // Write phase-space data
  FILE* file = fopen (fn1, "w");
  for (int i = 0; i < N; i++)
    fprintf (file, "%e %e\n", r(i), v(i));
  fclose (file);

  // Write electric field data
  Array<double,1> ne(J), n(J), phi(J), E(J);
  Density (r, ne);
  for (int j = 0; j < J; j++)
    n(j) = double (J) * ne(j) / double (N) - 1.;
  double kappa = 2. * M_PI / L;
  Poisson1D (phi, n, kappa);
  Electric (phi, E);

  file = fopen (fn2, "w");
  for (int j = 0; j < J; j++)
    {
      double x = double (j) * L / double (J);
      fprintf (file, "%e %e %e %e\n", x, ne(j), n(j), E(j));
    }
  double x = L;
  fprintf (file, "%e %e %e %e\n", x, ne(0), n(0), E(0));
  fclose (file);
}
The following routine returns a random velocity distributed on a double Maxwellian distribution function corresponding to two counter-streaming beams. The algorithm used to achieve this is called the rejection method, and will be discussed later in this course.

// Function to distribute electron velocities randomly so as
// to generate two counter propagating warm beams of thermal
// velocities unity and mean velocities +/- vb.
// Uses rejection method.

double distribution (double vb)
{
  // Initialize random number generator
  static int flag = 0;
  if (flag == 0)
    {
      int seed = time (NULL);
      srand (seed);
      flag = 1;
    }

  // Generate random v value
  double fmax = 0.5 * (1. + exp (-2. * vb * vb));
  double vmin = - 5. * vb;
  double vmax = + 5. * vb;
  double v = vmin + (vmax - vmin) * double (rand ()) / double (RAND_MAX);

  // Accept/reject value
  double f = 0.5 * (exp (-(v - vb) * (v - vb) / 2.) +
		    exp (-(v + vb) * (v + vb) / 2.));
  double x = fmax * double (rand ()) / double (RAND_MAX);
  if (x > f) return distribution (vb);
  else return v;
}
The routine below evaluates the electron number density on an evenly spaced mesh given the instantaneous electron coordinates.

// Evaluates electron number density n(0:J-1) from
// array r(0:N-1) of electron coordinates.

void Density (Array<double,1> r, Array<double,1>& n)
{
  // Initialize
  double dx = L / double (J);
  n = 0.;

  // Evaluate number density.
  for (int i = 0; i < N; i++)
    {
      int j = int (r(i) / dx);
      double y = r(i) / dx - double (j);
      n(j) += (1. - y) / dx;
      if (j+1 == J) n(0) += y / dx;
      else n(j+1) += y / dx;
    }
}
The following functions are wrapper routines for using the fftw library with periodic functions.

// Functions to calculate Fourier transforms of real data
// using fftw Fast-Fourier-Transform routine.
// Input/ouput arrays are assumed to be of extent J.

// Calculates Fourier transform of array f in arrays Fr and Fi
void fft_forward (Array<double,1>f, Array<double,1>&Fr,
      Array<double,1>& Fi)
{
  fftw_complex ff[J], FF[J];

  // Load data
  for (int j = 0; j < J; j++)
    {
      c_re (ff[j]) = f(j); c_im (ff[j]) = 0.;
    }

  // Call fftw routine
  fftw_plan p = fftw_create_plan (J, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_one (p, ff, FF);
  fftw_destroy_plan (p);

  // Unload data
  for (int j = 0; j < J; j++)
    {
      Fr(j) = c_re (FF[j]); Fi(j) = c_im (FF[j]);
    }

  // Normalize data
  Fr /= double (J);
  Fi /= double (J);
}

// Calculates inverse Fourier transform of arrays Fr and Fi in array f
void fft_backward (Array<double,1> Fr, Array<double,1> Fi,
      Array<double,1>& f)
{
  fftw_complex ff[J], FF[J];

  // Load data
  for (int j = 0; j < J; j++)
    {
      c_re (FF[j]) = Fr(j); c_im (FF[j]) = Fi(j);
    }

  // Call fftw routine
  fftw_plan p = fftw_create_plan (J, FFTW_BACKWARD, FFTW_ESTIMATE);
  fftw_one (p, FF, ff);
  fftw_destroy_plan (p);

  // Unload data
  for (int j = 0; j < J; j++)
      f(j) = c_re (ff[j]);
}
The following routine solves Poisson's equation in 1-D to find the instantaneous electric potential on a uniform grid.

// Solves 1-d Poisson equation:
//    d^u / dx^2 = v   for  0 <= x <= L
// Periodic boundary conditions:
//    u(x + L) = u(x),  v(x + L) = v(x)
// Arrays u and v assumed to be of length J.
// Now, jth grid point corresponds to
//    x_j = j dx  for j = 0,J-1
// where dx = L / J.
// Also,
//    kappa = 2 pi / L

void Poisson1D (Array<double,1>& u, Array<double,1> v, double kappa)
{
  // Declare local arrays.
  Array<double,1> Vr(J), Vi(J), Ur(J), Ui(J);

  // Fourier transform source term
  fft_forward (v, Vr, Vi);

  // Calculate Fourier transform of u
  Ur(0) = Ui(0) = 0.;
  for (int j = 1; j <= J/2; j++)
    {
      Ur(j) = - Vr(j) / double (j * j) / kappa / kappa;
      Ui(j) = - Vi(j) / double (j * j) / kappa / kappa;
    }
  for (int j = J/2; j < J; j++)
    {
      Ur(j) = Ur(J-j);
      Ui(j) = - Ui(J-j);
    }

  // Inverse Fourier transform to obtain u
  fft_backward (Ur, Ui, u);
}
The following function evaluates the electric field on a uniform grid from the electric potential.

// Calculate electric field from potential

void Electric (Array<double,1> phi, Array<double,1>& E)
{
  double dx = L / double (J);

  for (int j = 1; j < J-1; j++)
    E(j) = (phi(j-1) - phi(j+1)) / 2. / dx;
  E(0) = (phi(J-1) - phi(1)) / 2. / dx;
  E(J-1) = (phi(J-2) - phi(0)) / 2. / dx;
}
The following routine is the right-hand side routine for the electron equations of motion. Is is designed to be used with the fixed-step RK4 solver described earlier in this course.

// Electron equations of motion:
//    y(0:N-1)  = r_i
//    y(N:2N-1) = dr_i/dt

void rhs_eval (double t, Array<double,1> y, Array<double,1>& dydt)
{
  // Declare local arrays
  Array<double,1> r(N), v(N), rdot(N), vdot(N), r0(N);
  Array<double,1> ne(J), rho(J), phi(J), E(J);

  // Unload data from y
  UnLoad (y, r, v);

  // Make sure all coordinates in range 0 to L
  r0 = r;
  for (int i = 0; i < N; i++)
    {
      if (r0(i) < 0.) r0(i) += L;
      if (r0(i) > L) r0(i) -= L;
    }

  // Calculate electron number density
  Density (r0, ne);

  // Solve Poisson's equation
  double n0 = double (N) / L;
  for (int j = 0; j < J; j++)
    rho(j) = ne(j) / n0 - 1.;
  double kappa = 2. * M_PI / L;
  Poisson1D (phi, rho, kappa);

  // Calculate electric field
  Electric (phi, E);

  // Equations of motion
  for (int i = 0; i < N; i++)
    {
      double dx = L / double (J);
      int j = int (r0(i) / dx);
      double y = r0(i) / dx - double (j);

      double Efield;
      if (j+1 == J)
         Efield = E(j) * (1. - y) + E(0) * y;
      else
         Efield = E(j) * (1. - y) + E(j+1) * y;

      rdot(i) = v(i);
      vdot(i) = - Efield;
    }

  // Load data into dydt
  Load (rdot, vdot, dydt);
}
The following functions load and unload the electron phase-space coordinates into the solution vector y used by the RK4 routine.

// Load particle coordinates into solution vector

void Load (Array<double,1> r, Array<double,1> v, Array<double,1>& y)
{
  for (int i = 0; i < N; i++)
    {
      y(i) = r(i);
      y(N+i) = v(i);
    }
}

// Unload particle coordinates from solution vector

void UnLoad (Array<double,1> y, Array<double,1>& r, Array<double,1>& v)
{
  for (int i = 0; i < N; i++)
    {
      r(i) = y(i);
      v(i) = y(N+i);
    }
}


#include <boost/multiprecision/cpp_dec_float.hpp>
#include <iostream>
#include <cmath>
#include <iomanip>

const int precision = 100; // CHANGE FOR HIGHER PRECISION
typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<precision> > arbFloat;

// Prototypes
bool isStringValid(std::string str);
bool isNumberValid(arbFloat x);

int main() {
    // Multi-precision float declaration
    arbFloat wnew = 0.0;

    // Input and check string input
    std::cout << "0 <= x <= 1e9\nW(x), x = ";
    std::string inputStr;
    std::cin >> inputStr;
    if(!isStringValid(inputStr)) return 2;

    // Convert to arbFloat and check number
    arbFloat input = static_cast<arbFloat>(inputStr);
    if(!isNumberValid(input)) return 3;

    std::cout << std::setprecision(precision) << "\nConvergence:\n";

    std::string firstStr, secondStr;

    // Calculations
    int i;
    for(i = 0; i <= 1e99; i++){
        firstStr = wnew.convert_to<std::string>();
        firstStr.resize(precision + 2);

        wnew = ((wnew * wnew) + input * exp(-wnew))/(1+wnew);

        secondStr = wnew.convert_to<std::string>();
        secondStr.resize(precision + 2);

        std::cout << '\t' << wnew << '\n';

        if(firstStr == secondStr)
            break;
    }

    // Print result
    std::cout << "\nW(" << input << ") = " << wnew << "\n\n(rounded up to "
            << precision << " digits, precise after " << i << " iterations)\n";

    return 0;
}

// Check input
bool isStringValid(std::string str){
    // Check string containing multiple .
    std::size_t pos = str.find('.', 0);
    if(pos != std::string::npos){
        pos = str.find('.', pos+1);
        if(pos != std::string::npos){
            std::cout << "\nError: Multiple decimal marks\n";
            return false;
        }
    }

    // Check if NaN
    try{
        std::stod(str);
    } catch(...){
        std::cout << "\nError: NaN\n";
    }

    return true;
}

// Check number
bool isNumberValid(arbFloat x){
    // Check if result is imaginary
    if(x < 0){
        std::cout << "\nError: Imaginary result\n";
        return false;
    }

    // Range check
    if(x > 1e9){
        std::cout << "\nError: Out of bounds\n";
        return false;
    }

    return true;
}


#include <iostream>
#include <iomanip>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/src/Geometry/AngleAxis.h>

using namespace std;
using namespace Eigen;

Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta);
bool isRotationMatirx(Eigen::Matrix3d R);
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R);

const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;

int main1(){
    double roll_deg = 0.5;      // 绕X轴
    double pitch_deg = 0.8;     // 绕Y轴
    double yaw_deg = 108.5;     // 绕Z轴

    // 转化为弧度
    double roll_arc = roll_deg * DEG_TO_ARC;    // 绕X轴
    double pitch_arc = pitch_deg * DEG_TO_ARC;  // 绕Y轴
    double yaw_arc = yaw_deg * DEG_TO_ARC;      // 绕Z轴

    roll_arc =  1.12854674230422;    // 绕X轴
    pitch_arc = 1.08062392168743;  // 绕Y轴
    yaw_arc =   1.10953306595605;      // 绕Z轴

}

int main()
{
//    真值获取
    double lc_r = 0.015;
    double lc_p = 0.032;
    double lc_y = 0.061;

    double t_lc_x = 0.04;
    double t_lc_y = 0.08;
    double t_lc_z = -0.061;

    Eigen::AngleAxisd roll(AngleAxisd(lc_r, Vector3d::UnitZ()));
    Eigen::AngleAxisd pitch(AngleAxisd(-lc_p, Vector3d::UnitX()));
    Eigen::AngleAxisd yaw(AngleAxisd(-lc_y, Vector3d::UnitY()));
    Eigen::Matrix3d R_base;
    R_base<<
           0, -1, 0,
        0, 0, -1,
        1, 0,  0;
    Matrix3d gt_R_lc;
    gt_R_lc = roll * yaw * pitch;

//0.0150279
//-0.0609931
//-0.0329161

    Vector3d gt_t_lc = R_base * Eigen::Vector3d(t_lc_x, t_lc_y, t_lc_z);
    Matrix3d gt_R_cl = gt_R_lc.transpose();
    Vector3d gt_t_cl = -gt_R_cl * gt_t_lc;
    cout <<setprecision(15)<<"gt_rpy_cl:\n"<<gt_R_cl.eulerAngles(2,1,0)<<endl;
    cout <<"gt_R_cl:\n"<<gt_R_cl<<endl;
    cout <<"gt_t_cl:\n"<<gt_t_cl<<endl;

    // 转化为弧度
    double roll_arc = gt_R_cl.eulerAngles(2,1,0).z();    // 绕X轴
    double pitch_arc = gt_R_cl.eulerAngles(2,1,0).y();  // 绕Y轴
    double yaw_arc = gt_R_cl.eulerAngles(2,1,0).x();      // 绕Z轴

    cout << endl;
    cout << "roll_arc = " << roll_arc << endl;
    cout << "pitch_arc = " << pitch_arc << endl;
    cout << "yaw_arc = " << yaw_arc << endl;

    // 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度
    Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);

    // 使用Eigen库将欧拉角转换为旋转矩阵
    Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
    rotation_matrix1 = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());
    cout << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;

    // 使用自定义函数将欧拉角转换为旋转矩阵
    rotation_matrix2 = eulerAnglesToRotationMatrix(euler_angle);
    cout << "rotation matrix2 =\n" << rotation_matrix2 << endl << endl;

    // 使用Eigen将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2,1,0); // ZYX顺序，yaw,pitch,roll
    cout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1]
         << " " << eulerAngle1[0] << endl << endl;

    // 使用自定义函数将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yaw
    cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]
         << " " << eulerAngle2[2] << endl << endl;

    return 0;
}

Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta)
{
    Eigen::Matrix3d R_x;    // 计算旋转矩阵的X分量
    R_x <<
        1,              0,               0,
        0,  cos(theta[0]),  -sin(theta[0]),
        0,  sin(theta[0]),   cos(theta[0]);

    Eigen::Matrix3d R_y;    // 计算旋转矩阵的Y分量
    R_y <<
        cos(theta[1]),   0, sin(theta[1]),
        0,   1,             0,
        -sin(theta[1]),  0, cos(theta[1]);

    Eigen::Matrix3d R_z;    // 计算旋转矩阵的Z分量
    R_z <<
        cos(theta[2]), -sin(theta[2]), 0,
        sin(theta[2]),  cos(theta[2]), 0,
        0,              0,             1;
    Eigen::Matrix3d R = R_z * R_y * R_x;
    return R;
}

bool isRotationMatirx(Eigen::Matrix3d R)
{
    double err=1e-6;
    Eigen::Matrix3d shouldIdenity;
    shouldIdenity=R*R.transpose();
    Eigen::Matrix3d I=Eigen::Matrix3d::Identity();
    return (shouldIdenity - I).norm() < err;
}

Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
    assert(isRotationMatirx(R));
    double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular)
    {
        x = atan2( R(2,1), R(2,2));
        y = atan2(-R(2,0), sy);
        z = atan2( R(1,0), R(0,0));
    }
    else
    {
        x = atan2(-R(1,2), R(1,1));
        y = atan2(-R(2,0), sy);
        z = 0;
    }
    return {x, y, z};
}


/**
 * Initializing the location of data points in the low-D space
 */
#include <iostream>
#include <armadillo>
#include <chrono>
#include <omp.h>
#include <Eigen/Sparse>

#define SCALE 0.0001

using namespace std;
using namespace arma;
using namespace Eigen;

void Initialization (bool randominitializing, double** embedding, ofstream& logFile, int N, SparseMatrix<float> & graphSM, float MaxWeight, int DimLowSpace, int n_epochs){

	srand(17);
	/**
	 *  zero approximation in float precision
	 */
	float epsilon=1e-6;
	/**
	 * By deafult, low-D space dimensions are between -10 and 10
	 */
	int minDimLowDSpace=-10;
	int maxDimLowDSpace=10;

	if (!randominitializing){
		try{
			logFile<<" Spectral Initialization of Data in Lower Space"<<endl;
			cout<<" Spectral Initialization of Data in Lower Space"<<endl;

			/**
			 * graph is undirected weights (similarities) function for all the edges in the high-D space
			 */
			float** graph = new float*[N];
			for (int i = 0; i < N; ++i) { graph[i] = new float[N]; }

            #pragma omp parallel for
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					graph[i][j]=0;
				}
			}

			for (int k=0; k<graphSM.outerSize(); ++k){
				for (SparseMatrix<float>::InnerIterator it(graphSM,k); it; ++it) {
					graph[it.row()][it.col()]=it.value();
				}
			}

			/**
			 * Removing the small weights in accordance to https://github.com/lmcinnes/umap/blob/master/umap/umap_.py#L1032
			 */
            #pragma omp parallel for
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					if (graph[i][j] <  epsilon) continue;
					if (graph[i][j] <  MaxWeight/n_epochs) graph[i][j]=0;
				}
			}

			/**
			 * DegreeMatrix is a diagonal matrix contains information about the degree of each vertex
			 * sqrtDegreeMatrix transforms the diagonal values of DegreeMatrix by 1.0/sqrt()
			 */
			float** sqrtDegreeMatrix = new float*[N];
			for (int i = 0; i < N; ++i) { sqrtDegreeMatrix[i] = new float[N]; }

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

			for (int i = 0; i < N; ++i) {
				float sum=0;
				for (int j = 0; j < N; ++j) {
					sum+=graph[i][j];
				}
				sqrtDegreeMatrix[i][i]=1.0/sqrt(sum);
			}
			/**
			 * aux_mem is the column-wise transformation of sqrtDegreeMatrix as needed by armadillo function fmat
			 */
			float* aux_mem = new float[N*N];
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					aux_mem[j*N+i]=sqrtDegreeMatrix[i][j];
				}
			}
			delete [] sqrtDegreeMatrix;
			/**
			 * Making an armadillo sparse matrix spmatDegreeMatrix from sqrtDegreeMatrix
			 */
			fmat matDegreeMatrix(aux_mem,N,N,false,true);
			sp_fmat spmatDegreeMatrix(matDegreeMatrix);
			/**
			 * aux_mem2 is the column-wise transformation of adjacencyMatrix as needed by armadillo function fmat
			 */
			float* aux_mem2 = new float[N*N];
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					aux_mem2[j*N+i]=graph[i][j];   //column-wise
				}
			}

			delete[] graph;

			/**
			 * Making an armadillo sparse matrix spmatadjacencyMatrix from adjacencyMatrix
			 */
			fmat matadjacencyMatrix(aux_mem2,N,N,false,true);
			sp_fmat spmatadjacencyMatrix(matadjacencyMatrix);
			/**
			 * Making an armadillo sparse matrix of identity
			 */
			sp_fmat Unity = speye<sp_fmat>(N,N);
			/**
			 * Making an armadillo sparse matrix of Laplacian
			 */
			sp_fmat laplacianMatrix;
			laplacianMatrix= Unity-spmatDegreeMatrix*spmatadjacencyMatrix*spmatDegreeMatrix;
			/**
			 * Solving eigenvalue and eigenvector for Laplacian matrix
			 */
			fvec eigval;
			fmat eigvec;
			eigs_sym(eigval, eigvec, laplacianMatrix, DimLowSpace+1 , "sm");
			/**
			 * Converting eigenvectors to tmpvector
			 * will throw "error: Mat::col(): index out of bounds" if no eigvec was available
			 */
			typedef std::vector<float> stdvec;
			std::vector< std::vector<float> > tmpvector;

			for (int i = 1; i < DimLowSpace+1; ++i) {
				stdvec vectest = arma::conv_to< stdvec >::from(eigvec.col(i));
				tmpvector.push_back(vectest);
			}
			/**
			 * using tmpvector to intialize the locations of the points in low-D space
			 * embedding should not be outside the chosen dimensions for low-D space
			 */
			double maxembedding=0;
			for (int j = 0; j < DimLowSpace; ++j) {
				for (int i = 0; i < N; ++i) {
					double tmp=tmpvector[j][i];
					embedding[i][j]= tmp;

					if (abs(tmp) > maxembedding) maxembedding=tmp;
				}
			}

			double expansion=double(maxDimLowDSpace)/maxembedding;

			// Also adding a noise as prescribed in https://github.com/lmcinnes/umap/blob/master/umap/umap_.py#L1040
			unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
			std::default_random_engine generator (seed);
			std::normal_distribution<double> distribution (0.0,1.0);

			for (int i = 0; i < N; ++i) {
				for (int j = 0; j < DimLowSpace; ++j) {
					embedding[i][j] =embedding[i][j]* expansion+ SCALE*distribution(generator);
				}
			}

		} catch(std::exception& e){
			logFile<<" Spectral Initialization Failed. Will proceed with random initialization."<<endl;
			cout<<" Spectral Initialization Failed. Will proceed with random initialization."<<endl;
			randominitializing=true ; }
	}
	/**
	 * If the above procedure fails or randominitializing=1 as an input argument, the
	 * location of the points are determined randomly
	 */
	if (randominitializing){
		logFile<<" Random Initialization of Data in low-D Space"<<endl;
		cout<<" Random Initialization of Data in low-D Space"<<endl;
		for (int i = 0; i < N; ++i) {
			for (int j = 0; j < DimLowSpace; ++j) {
				double tmp=(double)rand()/RAND_MAX;
				embedding[i][j]=minDimLowDSpace+(maxDimLowDSpace-minDimLowDSpace)*tmp;
			}
		}
	}

	return;
}