// -*- c++ -*-
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>

#include <boost/graph/adjacency_list.hpp>

/*
  Thanks to Dale Gerdemann for this example, which inspired some
  changes to adjacency_list to make this work properly.
 */

/*
  Sample output:

  0  --c--> 1   --j--> 1   --c--> 2   --x--> 2
  1  --c--> 2   --d--> 3
  2  --t--> 4
  3  --h--> 4
  4

  merging vertex 1 into vertex 0

  0  --c--> 0   --j--> 0   --c--> 1   --x--> 1   --d--> 2
  1  --t--> 3
  2  --h--> 3
  3
 */

// merge_vertex(u,v,g):
// incoming/outgoing edges for v become incoming/outgoing edges for u
// v is deleted
template < class Graph, class GetEdgeProperties >
void merge_vertex(typename boost::graph_traits< Graph >::vertex_descriptor u,
    typename boost::graph_traits< Graph >::vertex_descriptor v, Graph& g,
    GetEdgeProperties getp)
{
    typedef boost::graph_traits< Graph > Traits;
    typename Traits::edge_descriptor e;
    typename Traits::out_edge_iterator out_i, out_end;
    for (boost::tie(out_i, out_end) = out_edges(v, g); out_i != out_end;
         ++out_i)
    {
        e = *out_i;
        typename Traits::vertex_descriptor targ = target(e, g);
        add_edge(u, targ, getp(e), g);
    }
    typename Traits::in_edge_iterator in_i, in_end;
    for (boost::tie(in_i, in_end) = in_edges(v, g); in_i != in_end; ++in_i)
    {
        e = *in_i;
        typename Traits::vertex_descriptor src = source(e, g);
        add_edge(src, u, getp(e), g);
    }
    clear_vertex(v, g);
    remove_vertex(v, g);
}

template < class StoredEdge > struct order_by_name
{
    typedef StoredEdge first_argument_type;
    typedef StoredEdge second_argument_type;
    typedef bool result_type;
    bool operator()(const StoredEdge& e1, const StoredEdge& e2) const
    {
        // Using std::pair operator< as an easy way to get lexicographical
        // compare over tuples.
        return std::make_pair(e1.get_target(), boost::get(boost::edge_name, e1))
            < std::make_pair(e2.get_target(), boost::get(boost::edge_name, e2));
    }
};
struct ordered_set_by_nameS
{
};

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
namespace boost
{
template < class ValueType >
struct container_gen< ordered_set_by_nameS, ValueType >
{
    typedef std::set< ValueType, order_by_name< ValueType > > type;
};
template <> struct parallel_edge_traits< ordered_set_by_nameS >
{
    typedef allow_parallel_edge_tag type;
};
}
#endif

template < class Graph > struct get_edge_name
{
    get_edge_name(const Graph& g_) : g(g_) {}

    template < class Edge >
    boost::property< boost::edge_name_t, char > operator()(Edge e) const
    {
        return boost::property< boost::edge_name_t, char >(
            boost::get(boost::edge_name, g, e));
    }
    const Graph& g;
};

int main()
{
#ifdef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    std::cout << "This program requires partial specialization." << std::endl;
#else
    using namespace boost;
    typedef property< edge_name_t, char > EdgeProperty;
    typedef adjacency_list< ordered_set_by_nameS, vecS, bidirectionalS,
        no_property, EdgeProperty >
        graph_type;

    graph_type g;

    add_edge(0, 1, EdgeProperty('j'), g);
    add_edge(0, 2, EdgeProperty('c'), g);
    add_edge(0, 2, EdgeProperty('x'), g);
    add_edge(1, 3, EdgeProperty('d'), g);
    add_edge(1, 2, EdgeProperty('c'), g);
    add_edge(1, 3, EdgeProperty('d'), g);
    add_edge(2, 4, EdgeProperty('t'), g);
    add_edge(3, 4, EdgeProperty('h'), g);
    add_edge(0, 1, EdgeProperty('c'), g);

    property_map< graph_type, vertex_index_t >::type id = get(vertex_index, g);
    property_map< graph_type, edge_name_t >::type name = get(edge_name, g);

    graph_traits< graph_type >::vertex_iterator i, end;
    graph_traits< graph_type >::out_edge_iterator ei, edge_end;

    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        std::cout << id[*i] << " ";
        for (boost::tie(ei, edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
            std::cout << " --" << name[*ei] << "--> " << id[target(*ei, g)]
                      << "  ";
        std::cout << std::endl;
    }
    std::cout << std::endl;

    std::cout << "merging vertex 1 into vertex 0" << std::endl << std::endl;
    merge_vertex(0, 1, g, get_edge_name< graph_type >(g));

    for (boost::tie(i, end) = vertices(g); i != end; ++i)
    {
        std::cout << id[*i] << " ";
        for (boost::tie(ei, edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
            std::cout << " --" << name[*ei] << "--> " << id[target(*ei, g)]
                      << "  ";
        std::cout << std::endl;
    }
    std::cout << std::endl;
#endif
    return 0;
}


/**
 * @file cosine_tree.hpp
 * @author Siddharth Agrawal
 *
 * Definition of Cosine Tree.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_CORE_TREE_COSINE_TREE_COSINE_TREE_HPP
#define MLPACK_CORE_TREE_COSINE_TREE_COSINE_TREE_HPP

#include <mlpack/prereqs.hpp>
#include <boost/heap/priority_queue.hpp>

namespace mlpack {
namespace tree {

// Predeclare classes for CosineNodeQueue typedef.
class CompareCosineNode;
class CosineTree;

// CosineNodeQueue typedef.
typedef boost::heap::priority_queue<CosineTree*,
    boost::heap::compare<CompareCosineNode> > CosineNodeQueue;

class CosineTree
{
 public:
  /**
   * CosineTree constructor for the root node of the tree. It initializes the
   * necessary variables required for splitting of the node, and building the
   * tree further. It takes a pointer to the input matrix and calculates the
   * relevant variables using it.
   *
   * @param dataset Matrix for which cosine tree is constructed.
   */
  CosineTree(const arma::mat& dataset);

  /**
   * CosineTree constructor for nodes other than the root node of the tree. It
   * takes in a pointer to the parent node and a list of column indices which
   * mentions the columns to be included in the node. The function calculate the
   * relevant variables just like the constructor above.
   *
   * @param parentNode Pointer to the parent cosine node.
   * @param subIndices Pointer to vector of column indices to be included.
   */
  CosineTree(CosineTree& parentNode, const std::vector<size_t>& subIndices);

  /**
   * Construct the CosineTree and the basis for the given matrix, and passed
   * 'epsilon' and 'delta' parameters. The CosineTree is constructed by
   * splitting nodes in the direction of maximum error, stored using a priority
   * queue. Basis vectors are added from the left and right children of the
   * split node. The basis vector from a node is the orthonormalized centroid of
   * its columns. The splitting continues till the Monte Carlo estimate of the
   * input matrix's projection on the obtained subspace is less than a fraction
   * of the norm of the input matrix.
   *
   * @param dataset Matrix for which the CosineTree is constructed.
   * @param epsilon Error tolerance fraction for calculated subspace.
   * @param delta Cumulative probability for Monte Carlo error lower bound.
   */
  CosineTree(const arma::mat& dataset,
             const double epsilon,
             const double delta);

  /**
   * Clean up the CosineTree: release allocated memory (including children).
   */
  ~CosineTree();

  /**
   * Calculates the orthonormalization of the passed centroid, with respect to
   * the current vector subspace.
   *
   * @param treeQueue Priority queue of cosine nodes.
   * @param centroid Centroid of the node being added to the basis.
   * @param newBasisVector Orthonormalized centroid of the node.
   * @param addBasisVector Address to additional basis vector.
   */
  void ModifiedGramSchmidt(CosineNodeQueue& treeQueue,
                           arma::vec& centroid,
                           arma::vec& newBasisVector,
                           arma::vec* addBasisVector = NULL);

  /**
   * Estimates the squared error of the projection of the input node's matrix
   * onto the current vector subspace. A normal distribution is fit using
   * weighted norms of projections of samples drawn from the input node's matrix
   * columns. The error is calculated as the difference between the Frobenius
   * norm of the input node's matrix and lower bound of the normal distribution.
   *
   * @param node Node for which Monte Carlo estimate is calculated.
   * @param treeQueue Priority queue of cosine nodes.
   * @param addBasisVector1 Address to first additional basis vector.
   * @param addBasisVector2 Address to second additional basis vector.
   */
  double MonteCarloError(CosineTree* node,
                         CosineNodeQueue& treeQueue,
                         arma::vec* addBasisVector1 = NULL,
                         arma::vec* addBasisVector2 = NULL);

  /**
   * Constructs the final basis matrix, after the cosine tree construction.
   *
   * @param treeQueue Priority queue of cosine nodes.
   */
  void ConstructBasis(CosineNodeQueue& treeQueue);

  /**
   * This function splits the cosine node into two children based on the cosines
   * of the columns contained in the node, with respect to the sampled splitting
   * point. The function also calls the CosineTree constructor for the children.
   */
  void CosineNodeSplit();

  /**
   * Sample 'numSamples' points from the Length-Squared distribution of the
   * cosine node. The function uses 'l2NormsSquared' to calculate the cumulative
   * probability distribution of the column vectors. The sampling is based on a
   * randomly generated values in the range [0, 1].
   */
  void ColumnSamplesLS(std::vector<size_t>& sampledIndices,
                       arma::vec& probabilities, size_t numSamples);

  /**
   * Sample a point from the Length-Squared distribution of the cosine node. The
   * function uses 'l2NormsSquared' to calculate the cumulative probability
   * distribution of the column vectors. The sampling is based on a randomly
   * generated value in the range [0, 1].
   */
  size_t ColumnSampleLS();

  /**
   * Sample a column based on the cumulative Length-Squared distribution of the
   * cosine node, and a randomly generated value in the range [0, 1]. Binary
   * search is more efficient than searching linearly for the same. This leads
   * a significant speedup when there are large number of columns to choose from
   * and when a number of samples are to be drawn from the distribution.
   *
   * @param cDistribution Cumulative LS distribution of columns in the node.
   * @param value Randomly generated value in the range [0, 1].
   * @param start Starting index of the distribution interval to search in.
   * @param end Ending index of the distribution interval to search in.
   */
  size_t BinarySearch(arma::vec& cDistribution, double value, size_t start,
                      size_t end);

  /**
   * Calculate cosines of the columns present in the node, with respect to the
   * sampled splitting point. The calculated cosine values are useful for
   * splitting the node into its children.
   *
   * @param cosines Vector to store the cosine values in.
   */
  void CalculateCosines(arma::vec& cosines);

  /**
   * Calculate centroid of the columns present in the node. The calculated
   * centroid is used as a basis vector for the cosine tree being constructed.
   */
  void CalculateCentroid();

  //! Returns the basis of the constructed subspace.
  void GetFinalBasis(arma::mat& finalBasis) { finalBasis = basis; }

  //! Get pointer to the dataset matrix.
  const arma::mat& GetDataset() const { return dataset; }

  //! Get the indices of columns in the node.
  std::vector<size_t>& VectorIndices() { return indices; }

  //! Set the Monte Carlo error.
  void L2Error(const double error) { this->l2Error = error; }
  //! Get the Monte Carlo error.
  double L2Error() const { return l2Error; }

  //! Get pointer to the centroid vector.
  arma::vec& Centroid() { return centroid; }

  //! Set the basis vector of the node.
  void BasisVector(arma::vec& bVector) { this->basisVector = bVector; }

  //! Get the basis vector of the node.
  arma::vec& BasisVector() { return basisVector; }

  //! Get pointer to the parent node.
  CosineTree* Parent() const { return parent; }
  //! Modify the pointer to the parent node.
  CosineTree*& Parent() { return parent; }

  //! Get pointer to the left child of the node.
  CosineTree* Left() const { return left; }
  //! Modify the pointer to the left child of the node.
  CosineTree*& Left() { return left; }

  //! Get pointer to the right child of the node.
  CosineTree* Right() const { return right; }
  //! Modify the pointer to the left child of the node.
  CosineTree*& Right() { return right; }

  //! Get number of columns of input matrix in the node.
  size_t NumColumns() const { return numColumns; }

  //! Get the Frobenius norm squared of columns in the node.
  double FrobNormSquared() const { return frobNormSquared; }

  //! Get the column index of split point of the node.
  size_t SplitPointIndex() const { return indices[splitPointIndex]; }

 private:
  //! Matrix for which cosine tree is constructed.
  const arma::mat& dataset;
  //! Cumulative probability for Monte Carlo error lower bound.
  double delta;
  //! Subspace basis of the input dataset.
  arma::mat basis;
  //! Parent of the node.
  CosineTree* parent;
  //! Left child of the node.
  CosineTree* left;
  //! Right child of the node.
  CosineTree* right;
  //! Indices of columns of input matrix in the node.
  std::vector<size_t> indices;
  //! L2-norm squared of columns in the node.
  arma::vec l2NormsSquared;
  //! Centroid of columns of input matrix in the node.
  arma::vec centroid;
  //! Orthonormalized basis vector of the node.
  arma::vec basisVector;
  //! Index of split point of cosine node.
  size_t splitPointIndex;
  //! Number of columns of input matrix in the node.
  size_t numColumns;
  //! Monte Carlo error for this node.
  double l2Error;
  //! Frobenius norm squared of columns in the node.
  double frobNormSquared;
};

class CompareCosineNode
{
 public:
  // Comparison function for construction of priority queue.
  bool operator() (const CosineTree* a, const CosineTree* b) const
  {
    return a->L2Error() < b->L2Error();
  }
};

} // namespace tree
} // namespace mlpack

#endif


#include "polygons_to_triangles.h"
#include "ear_clipping.h"
#include "../sort.h"
#include "../slice.h"
#include <Eigen/Eigenvalues>

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename DerivedF,
  typename DerivedJ>
IGL_INLINE void igl::predicates::polygons_to_triangles(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedJ> & J)
{
  typedef Eigen::Index Index;
  // Each polygon results in #sides-2 triangles. So ∑#sides-2
  F.resize(C(C.size()-1) - (C.size()-1)*2,3);
  J.resize(F.rows());
  {
    Index f = 0;
    for(Index p = 0;p<C.size()-1;p++)
    {
      const Index np = C(p+1)-C(p);
      Eigen::MatrixXi pF;
      if(np == 3)
      {
        pF = (Eigen::MatrixXi(1,3)<<0,1,2).finished();
      }else
      {
        // Make little copy of this polygon with an initial fan
        DerivedV pV(np,V.cols());
        for(Index c = 0;c<np;c++)
        {
          pV.row(c) = V.row(I(C(p)+c));
        }
        // Use PCA to project to 2D
        Eigen::MatrixXd S;
        switch(V.cols())
        {
          case 2:
            S = V.template cast<double>();
            break;
          case 3:
          {
            Eigen::MatrixXd P = (pV.rowwise() - pV.colwise().mean()).template cast<double>();
            Eigen::Matrix3d O = P.transpose() * P;
            Eigen::EigenSolver<Eigen::Matrix3d> es(O);
            Eigen::Matrix3d C = es.eigenvectors().real();
            {
              Eigen::Vector3d _1;
              Eigen::Vector3i I;
              igl::sort(es.eigenvalues().real().eval(),1,false,_1,I);
              igl::slice(Eigen::Matrix3d(C),I,2,C);
            }
            S = P*C.leftCols(2);
            break;
          }
          default: assert(false && "dim>3 not supported");
        }

        Eigen::VectorXi RT = Eigen::VectorXi::Zero(S.rows(),1);
        Eigen::VectorXi _I;
        Eigen::MatrixXd _nS;

        // compute signed area
        {
          double area = 0;
          for(Index c = 0;c<np;c++)
          {
            area += S((c+0)%np,0)*S((c+1)%np,1) - S((c+1)%np,0)*S((c+0)%np,1);
          }
          //prIndexf("area: %g\n",area);
          if(area<0)
          {
            S.col(0) *= -1;
          }
        }

        // This is a really low quality triangulator and will contain nearly
        // degenerate elements which become degenerate or worse when unprojected
        // back to 3D.
        igl::predicates::ear_clipping(S,RT,_I,pF,_nS);
        // igl::predicates::ear_clipping does not gracefully fail when the input
        // is not simple. Instead it (tends?) to output too few triangles.
        if(pF.rows() < np-2)
        {
          // Fallback, use a fan
          //std::cout<<igl::matlab_format(S,"S")<<std::endl;
          //std::cout<<igl::matlab_format(RT,"RT")<<std::endl;
          //std::cout<<igl::matlab_format(_I,"I")<<std::endl;
          //std::cout<<igl::matlab_format(pF,"pF")<<std::endl;
          //std::cout<<igl::matlab_format(_nS,"nS")<<std::endl;
          //std::cout<<std::endl;

          pF.resize(np-2,3);
          for(Index c = 0;c<np;c++)
          {
            if(c>0 && c<np-1)
            {
              pF(c-1,0) = 0;
              pF(c-1,1) = c;
              pF(c-1,2) = c+1;
            }
          }
        }
        assert(pF.rows() == np-2);

        // Could at least flip edges of degenerate edges

        //if(pF.rows()>1)
        //{
        //  // Delaunay-ize
        //  Eigen::MatrixXd pl;
        //  igl::edge_lengths(pV,pF,pl);

        //  typedef Eigen::Matrix<Index,Eigen::Dynamic,2> MatrixX2I;
        //  typedef Eigen::Matrix<Index,Eigen::Dynamic,1> VectorXI;
        //  MatrixX2I E,uE;
        //  VectorXI EMAP;
        //  std::vector<std::vector<Index> > uE2E;
        //  igl::unique_edge_map(pF, E, uE, EMAP, uE2E);
        //  typedef Index Index;
        //  typedef double Scalar;
        //  const Index num_faces = pF.rows();
        //  std::vector<Index> Q;
        //  Q.reserve(uE2E.size());
        //  for (size_t uei=0; uei<uE2E.size(); uei++)
        //  {
        //    Q.push_back(uei);
        //  }
        //  while(!Q.empty())
        //  {
        //    const Index uei = Q.back();
        //    Q.pop_back();
        //    if (uE2E[uei].size() == 2)
        //    {
        //      double w;
        //      igl::is_Indexrinsic_delaunay(pl,uE2E,num_faces,uei,w);
        //      prIndexf("%d : %0.17f\n",uei,w);
        //      if(w<-1e-7)
        //      {
        //        prIndexf("  flippin'\n");
        //        //
        //        //          v1                 v1
        //        //          /|\                / \
        //        //        c/ | \b            c/f1 \b
        //        //     v3 /f2|f1\ v4  =>  v3 /__f__\ v4
        //        //        \  e  /            \ f2  /
        //        //        d\ | /a            d\   /a
        //        //          \|/                \ /
        //        //          v2                 v2
        //        //
        //        // hmm... is the flip actually in the other direction?
        //        const Index f1 = uE2E[uei][0]%num_faces;
        //        const Index f2 = uE2E[uei][1]%num_faces;
        //        const Index c1 = uE2E[uei][0]/num_faces;
        //        const Index c2 = uE2E[uei][1]/num_faces;
        //        const size_t e_24 = f1 + ((c1 + 1) % 3) * num_faces;
        //        const size_t e_41 = f1 + ((c1 + 2) % 3) * num_faces;
        //        const size_t e_13 = f2 + ((c2 + 1) % 3) * num_faces;
        //        const size_t e_32 = f2 + ((c2 + 2) % 3) * num_faces;
        //        const size_t ue_24 = EMAP(e_24);
        //        const size_t ue_41 = EMAP(e_41);
        //        const size_t ue_13 = EMAP(e_13);
        //        const size_t ue_32 = EMAP(e_32);
        //        // new edge lengths
        //        const Index v1 = pF(f1, (c1+1)%3);
        //        const Index v2 = pF(f1, (c1+2)%3);
        //        const Index v4 = pF(f1, c1);
        //        const Index v3 = pF(f2, c2);
        //        {
        //          const Scalar e = pl(f1,c1);
        //          const Scalar a = pl(f1,(c1+1)%3);
        //          const Scalar b = pl(f1,(c1+2)%3);
        //          const Scalar c = pl(f2,(c2+1)%3);
        //          const Scalar d = pl(f2,(c2+2)%3);
        //          const double f = (pV.row(v3)-pV.row(v4)).norm();
        //          // New order
        //          pl(f1,0) = f;
        //          pl(f1,1) = b;
        //          pl(f1,2) = c;
        //          pl(f2,0) = f;
        //          pl(f2,1) = d;
        //          pl(f2,2) = a;
        //        }
        //        prIndexf("%d,%d %d,%d -> %d,%d\n",uE(uei,0),uE(uei,1),v1,v2,v3,v4);
        //        igl::flip_edge(pF, E, uE, EMAP, uE2E, uei);
        //        std::cout<<"  "<<pl.row(f1)<<std::endl;
        //        std::cout<<"  "<<pl.row(f2)<<std::endl;
        //        //// new edge lengths, slow!
        //        //igl::edge_lengths(pV,pF,pl);
        //        // recompute edge lengths of two faces. (extra work on untouched
        //        // edges)
        //        for(Index f : {f1,f2})
        //        {
        //          for(Index c=0;c<3;c++)
        //          {
        //            pl(f,c) =
        //              (pV.row(pF(f,(c+1)%3))-pV.row(pF(f,(c+2)%3))).norm();
        //          }
        //        }
        //        std::cout<<"  "<<pl.row(f1)<<std::endl;
        //        std::cout<<"  "<<pl.row(f2)<<std::endl;
        //        std::cout<<std::endl;

        //        Q.push_back(ue_24);
        //        Q.push_back(ue_41);
        //        Q.push_back(ue_13);
        //        Q.push_back(ue_32);
        //      }
        //    }
        //  }

        //  // check for self-loops (I claim these cannot happen)
        //  for(Index f = 0;f<pF.rows();f++)
        //  {
        //    for(Index c =0;c<3;c++)
        //    {
        //      assert(pF(f,c) != pF(f,(c+1)%3) && "self loops should not exist");
        //    }
        //  }
        //}
      }
      // Copy Indexo global list
      for(Index i = 0;i<pF.rows();i++)
      {
        for(Index c =0;c<3;c++)
        {
          F(f,c) = I(C(p)+pF(i,c));
        }
        J(f) = p;
        f++;
      }

    }
    assert(f == F.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
#endif


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

/* Author: Mark Moll */

#include "ompl/base/spaces/ReedsSheppStateSpace.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/Exception.h"
#include <queue>
#include <boost/math/constants/constants.hpp>

using namespace ompl::base;

namespace
{
    // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.

    const double pi = boost::math::constants::pi<double>();
    const double twopi = 2. * pi;
    const double RS_EPS = 1e-6;
    const double ZERO = 10*std::numeric_limits<double>::epsilon();

    inline double mod2pi(double x)
    {
        double v = fmod(x, twopi);
        if (v < -pi)
            v += twopi;
        else
            if (v > pi)
                v -= twopi;
        return v;
    }
    inline void polar(double x, double y, double &r, double &theta)
    {
        r = sqrt(x*x + y*y);
        theta = atan2(y, x);
    }
    inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
    {
        double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
        double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
        tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
        omega = mod2pi(tau - u + v - phi) ;
    }

    // formula 8.1 in Reeds-Shepp paper
    inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
    {
        polar(x - sin(phi), y - 1. + cos(phi), u, t);
        if (t >= -ZERO)
        {
            v = mod2pi(phi - t);
            if (v >= -ZERO)
            {
                assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
                assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
                assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
                return true;
            }
        }
        return false;
    }
    // formula 8.2
    inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double t1, u1;
        polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
        u1 = u1*u1;
        if (u1 >= 4.)
        {
            double theta;
            u = sqrt(u1 - 4.);
            theta = atan2(2., u);
            t = mod2pi(t1 + theta);
            v = mod2pi(t - phi);
            assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
            assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
            return t>=-ZERO && v>=-ZERO;
        }
        return false;
    }
    void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
            Lmin = L;
        }
        if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
            Lmin = L;
        }
        if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
            Lmin = L;
        }
        if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
            Lmin = L;
        }
        if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
    }
    // formula 8.3 / 8.4  *** TYPO IN PAPER ***
    inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
        polar(xi, eta, u1, theta);
        if (u1 <= 4.)
        {
            u = -2.*asin(.25 * u1);
            t = mod2pi(theta + .5 * u + pi);
            v = mod2pi(phi - t + u);
            assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
            assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO;
        }
        return false;
    }
    void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
            Lmin = L;
        }
        if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
            Lmin = L;
        }
        if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
            Lmin = L;
        }
        if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
            Lmin = L;
        }

        // backwards
        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
        if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
            Lmin = L;
        }
        if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
            Lmin = L;
        }
        if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
            Lmin = L;
        }
        if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
    }
    // formula 8.7
    inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
        if (rho <= 1.)
        {
            u = acos(rho);
            tauOmega(u, -u, xi, eta, phi, t, v);
            assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
            assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
            return t>=-ZERO && v<=ZERO;
        }
        return false;
    }
    // formula 8.8
    inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
        if (rho>=0 && rho<=1)
        {
            u = -acos(rho);
            if (u >= -.5 * pi)
            {
                tauOmega(u, u, xi, eta, phi, t, v);
                assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
                assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
                assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
                return t>=-ZERO && v>=-ZERO;
            }
        }
        return false;
    }
    void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
            Lmin = L;
        }
        if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
            Lmin = L;
        }
        if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
            Lmin = L;
        }
        if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
            Lmin = L;
        }

        if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
            Lmin = L;
        }
        if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
            Lmin = L;
        }
        if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
            Lmin = L;
        }
        if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
    }
    // formula 8.9
    inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
        polar(xi, eta, rho, theta);
        if (rho >= 2.)
        {
            double r = sqrt(rho*rho - 4.);
            u = 2. - r;
            t = mod2pi(theta + atan2(r, -2.));
            v = mod2pi(phi - .5*pi - t);
            assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
            assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO && v<=ZERO;
        }
        return false;
    }
    // formula 8.10
    inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
        polar(-eta, xi, rho, theta);
        if (rho >= 2.)
        {
            t = theta;
            u = 2. - rho;
            v = mod2pi(t + .5*pi - phi);
            assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
            assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO && v<=ZERO;
        }
        return false;
    }
    void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length() - .5*pi, L;
        if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
            Lmin = L;
        }
        if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
            Lmin = L;
        }

        if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
            Lmin = L;
        }
        if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
            Lmin = L;
        }

        // backwards
        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
        if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
            Lmin = L;
        }
        if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
            Lmin = L;
        }

        if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
            Lmin = L;
        }
        if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
    }
    // formula 8.11 *** TYPO IN PAPER ***
    inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
        polar(xi, eta, rho, theta);
        if (rho >= 2.)
        {
            u = 4. - sqrt(rho*rho - 4.);
            if (u <= ZERO)
            {
                t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
                v = mod2pi(t - phi);
                assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
                assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
                assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
                return t>=-ZERO && v>=-ZERO;
            }
        }
        return false;
    }
    void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length() - pi, L;
        if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
            Lmin = L;
        }
        if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
            Lmin = L;
        }
        if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
            Lmin = L;
        }
        if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
    }

    ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
    {
        ReedsSheppStateSpace::ReedsSheppPath path;
        CSC(x, y, phi, path);
        CCC(x, y, phi, path);
        CCCC(x, y, phi, path);
        CCSC(x, y, phi, path);
        CCSCC(x, y, phi, path);
        return path;
    }
}

const ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
ompl::base::ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },             // 0
    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },            // 1
    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },           // 2
    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },           // 3
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 4
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 5
    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },        // 6
    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 7
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 8
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 9
    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },       // 10
    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },        // 11
    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },         // 12
    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 13
    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },          // 14
    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },        // 15
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },      // 16
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }       // 17
};

ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
    double t, double u, double v, double w, double x)
    : type_(type)
{
    length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
    totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
}

double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
{
    return rho_ * reedsShepp(state1, state2).length();
}

void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
{
    bool firstTime = true;
    ReedsSheppPath path;
    interpolate(from, to, t, firstTime, path, state);
}

void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
    bool &firstTime, ReedsSheppPath &path, State *state) const
{
    if (firstTime)
    {
        if (t>=1.)
        {
            if (to != state)
                copyState(state, to);
            return;
        }
        if (t<=0.)
        {
            if (from != state)
                copyState(state, from);
            return;
        }
        path = reedsShepp(from, to);
        firstTime = false;
    }
    interpolate(from, path, t, state);
}

void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath &path, double t, State *state) const
{
    StateType *s = allocState()->as<StateType>();
    double seg = t * path.length(), phi, v;

    s->setXY(0., 0.);
    s->setYaw(from->as<StateType>()->getYaw());
    for (unsigned int i=0; i<5 && seg>0; ++i)
    {
        if (path.length_[i]<0)
        {
            v = std::max(-seg, path.length_[i]);
            seg += v;
        }
        else
        {
            v = std::min(seg, path.length_[i]);
            seg -= v;
        }
        phi = s->getYaw();
        switch(path.type_[i])
        {
            case RS_LEFT:
                s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
                s->setYaw(phi+v);
                break;
            case RS_RIGHT:
                s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
                s->setYaw(phi-v);
                break;
            case RS_STRAIGHT:
                s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
                break;
            case RS_NOP:
                break;
        }
    }
    state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
    state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
    getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
    state->as<StateType>()->setYaw(s->getYaw());
    freeState(s);
}

ompl::base::ReedsSheppStateSpace::ReedsSheppPath ompl::base::ReedsSheppStateSpace::reedsShepp(const State *state1, const State *state2) const
{
    const StateType *s1 = static_cast<const StateType*>(state1);
    const StateType *s2 = static_cast<const StateType*>(state2);
    double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
    double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
    double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
    double x = c*dx + s*dy, y = -s*dx + c*dy, phi = th2 - th1;
    return ::reedsShepp(x/rho_, y/rho_, phi);
}

void ompl::base::ReedsSheppMotionValidator::defaultSettings()
{
    stateSpace_ = dynamic_cast<ReedsSheppStateSpace*>(si_->getStateSpace().get());
    if (!stateSpace_)
        throw Exception("No state space for motion validator");
}

bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
{
    /* assume motion starts in a valid configuration so s1 is valid */

    bool result = true, firstTime = true;
    ReedsSheppStateSpace::ReedsSheppPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    if (nd > 1)
    {
        /* temporary storage for the checked state */
        State *test = si_->allocState();

        for (int j = 1 ; j < nd ; ++j)
        {
            stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
            if (!si_->isValid(test))
            {
                lastValid.second = (double)(j - 1) / (double)nd;
                if (lastValid.first)
                    stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
                result = false;
                break;
            }
        }
        si_->freeState(test);
    }

    if (result)
        if (!si_->isValid(s2))
        {
            lastValid.second = (double)(nd - 1) / (double)nd;
            if (lastValid.first)
                stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
            result = false;
        }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}

bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2) const
{
    /* assume motion starts in a valid configuration so s1 is valid */
    if (!si_->isValid(s2))
        return false;

    bool result = true, firstTime = true;
    ReedsSheppStateSpace::ReedsSheppPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    /* initialize the queue of test positions */
    std::queue< std::pair<int, int> > pos;
    if (nd >= 2)
    {
        pos.push(std::make_pair(1, nd - 1));

        /* temporary storage for the checked state */
        State *test = si_->allocState();

        /* repeatedly subdivide the path segment in the middle (and check the middle) */
        while (!pos.empty())
        {
            std::pair<int, int> x = pos.front();

            int mid = (x.first + x.second) / 2;
            stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);

            if (!si_->isValid(test))
            {
                result = false;
                break;
            }

            pos.pop();

            if (x.first < mid)
                pos.push(std::make_pair(x.first, mid - 1));
            if (x.second > mid)
                pos.push(std::make_pair(mid + 1, x.second));
        }

        si_->freeState(test);
    }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_CBRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_CBRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/negate.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotomnmbo_3.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/is_less.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/logical_or.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(cbrt_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::single_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0 ) const BOOST_NOEXCEPT
      {
        A0 z =  bs::abs(a0);
        using int_type =  bd::as_integer_t<A0, signed>;
  #ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = is_less(z, Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
  #endif
        const A0 CBRT2  = Constant< A0, 0x3fa14518> ();
        const A0 CBRT4  = Constant< A0, 0x3fcb2ff5> ();
        const A0 CBRT2I = Constant< A0, 0x3f4b2ff5> ();
        const A0 CBRT4I = Constant< A0, 0x3f214518> ();
        int_type e;
        A0 x; std::tie(x, e) = fast_(frexp)(z);
        x = horn <A0,
          0x3ece0609,
          0x3f91eb77,
          0xbf745265,
          0x3f0bf0fe,
          0xbe09e49a
          > (x);
        auto flag = is_gez(e);
        int_type e1 =  bs::abs(e);
        int_type rem = e1;
        e1 /= Three<int_type>();
        rem -= e1*Three<int_type>();
        e = negate(e1, e);
        const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
        const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
        A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
        fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
        x = fast_(ldexp)(x*fact, e);
        x -= (x-z/sqr(x))*Third<A0>();
  #ifndef BOOST_SIMD_NO_DENORMALS
        x = bitwise_or(x, bitofsign(a0))*f;
  #else
        x = bitwise_or(x, bitofsign(a0));
  #endif
  #ifndef BOOST_SIMD_NO_INFINITIES
        return if_else(logical_or(is_eqz(a0),is_inf(a0)), a0, x);
  #else
        return if_else(is_eqz(a0), a0, x);
  #endif
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(cbrt_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::double_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0 ) const BOOST_NOEXCEPT
      {
        using int_type =  bd::as_integer_t<A0, signed>;
        A0 z =  bs::abs(a0);
     #ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = is_less(z, Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
     #endif
        const A0 CBRT2  = Constant< A0, 0x3ff428a2f98d728bll> ();
        const A0 CBRT4  = Constant< A0, 0x3ff965fea53d6e3dll> ();
        const A0 CBRT2I = Constant< A0, 0x3fe965fea53d6e3dll> ();
        const A0 CBRT4I = Constant< A0, 0x3fe428a2f98d728bll> ();
        int_type e;
        A0 x;
        std::tie(x, e) = fast_(frexp)(z);
        x = horn <A0,
          0x3fd9c0c12122a4fell,
          0x3ff23d6ee505873all,
          0xbfee8a4ca3ba37b8ll,
          0x3fe17e1fc7e59d58ll,
          0xbfc13c93386fdff6ll >  (x);
        auto flag = is_gez(e);
        int_type e1 =  bs::abs(e);
        int_type rem = e1;
        e1 /= Three<int_type>();
        rem -= e1*Three<int_type>();
        e =  negate(e1, e);
        const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
        const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
        A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
        fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
        x = fast_(ldexp)(x*fact, e);
        x -= (x-z/sqr(x))*Third<A0>();
        x -= (x-z/sqr(x))*Third<A0>(); //two newton passes
      #ifndef BOOST_SIMD_NO_DENORMALS
        x = bitwise_or(x, bitofsign(a0))*f;
      #else
        x = bitwise_or(x, bitofsign(a0));
      #endif
      #ifndef BOOST_SIMD_NO_INFINITIES
        return if_else(logical_or(is_eqz(a0),is_inf(a0)), a0, x);
      #else
        return if_else(is_eqz(a0), a0, x);
      #endif
    }
   };

}

} }

#endif


#define DEBUG 1
/**
 * File    : C.cpp
 * Author  : Kazune Takahashi
 * Created : 12/29/2019, 11:12:55 AM
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- main() -----

class Domino
{
  using board = vector<string>;
  using point = tuple<int, int>;
  vector<board> seed;

  int N;
  board answer;

public:
  Domino(int N) : N{N}
  {
    init_seed();
    stringstream SS;
    for (auto i = 0; i < N; i++)
    {
      SS << '.';
    }
    answer = board(N, SS.str());
  }

  void flush()
  {
    if (N == 2)
    {
      cout << "-1" << endl;
      return;
    }
    make_ans();
    assert(static_cast<int>(answer.size()) == N);
    for (auto i = 0; i < N; i++)
    {
      assert(static_cast<int>(answer[i].size()) == N);
      cout << answer[i] << endl;
    }
  }

  void init_seed()
  {
    seed.resize(8);
    seed[3] = {
        "aa.",
        "..a",
        "..a"};
    seed[4] = {
        "aabc",
        "ddbc",
        "bcaa",
        "bcdd"};
    seed[5] = {
        "aabba",
        "bcc.a",
        "b..cb",
        "a..cb",
        "abbaa"};
    seed[6] = {
        "aabc..",
        "ddbc..",
        "..aabc",
        "..ddbc",
        "bc..aa",
        "bc..dd"};
    seed[7] = {
        "aabbcc.",
        "dd.dd.a",
        "..d..da",
        "..d..db",
        "dd.dd.b",
        "..d..dc",
        "..d..dc"};
  }

private:
  void make_ans()
  {
    if (N == 3)
    {
      answer = seed[3];
      return;
    }
    int S{N % 4 + 4};
    int x{(N - S) / 4};
    for (auto k = 0; k < x; k++)
    {
      for (auto i = k * 4; i < (k + 1) * 4; i++)
      {
        for (auto j = k * 4; j < (k + 1) * 4; j++)
        {
          answer[i][j] = seed[4][i % 4][j % 4];
        }
      }
    }
    for (auto i = x * 4; i < N; i++)
    {
      for (auto j = x * 4; j < N; j++)
      {
        answer[i][j] = seed[S][i - x * 4][j - x * 4];
      }
    }
  }
};

int main()
{
  int N;
  cin >> N;
  Domino domino{N};
  domino.flush();
}


/*
 * LiquidSampler.cpp
 *
 *  Created on: Apr 18, 2012
 *      Author: selman.joe@gmail.com
 */

#include <cstdlib>
#include <vector>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/bernoulli_distribution.hpp>

#include "LiquidSampler.h"
#include "../SpanInterval.h"
#include "../SISet.h"

std::vector<SpanInterval> LiquidSampler::operator ()(const SpanInterval& si, double p, boost::mt19937& rng) const {
    std::vector<SpanInterval> sampled;

    if (p <= 0.0) return sampled;
    if (p >= 1.0) {
        sampled.push_back(si);
        return sampled;
    }

    if (si.isEmpty()) return sampled;

    // loop over intervals ranging from the width of si, down to 1
    std::list<Interval> toSample;
    toSample.push_front(Interval(si.start().start(), si.finish().finish()));
    do {
        Interval curInterval = toSample.front();
        toSample.pop_front();
        //bool sampleCurInterval = ((double)rand() / (double)RAND_MAX) <= p;
        boost::bernoulli_distribution<double> flip(p);
        bool sampleCurInterval = flip(rng);
        if (sampleCurInterval) {
            sampled.push_back(SpanInterval(curInterval));
            // now make sure none of our intervals overlap the sampled interval
            for (std::list<Interval>::iterator it = toSample.begin();
                    it != toSample.end();) {
                if (intersection(curInterval, *it)) {
                    std::vector<Interval> subtracted = it->subtract(curInterval);
                    // put what we just processed in there and erase it
                    for (std::vector<Interval>::const_iterator it2 = subtracted.begin();
                            it2 != subtracted.end();
                            it2++) {
                        toSample.insert(it, *it2);
                    }
                    it = toSample.erase(it);
                } else {
                    it++;
                }
            }
        } else {
            // didn't sample, so lets put the smaller size of intervals next in to sample
            if (curInterval.size() != 1) {
                Interval left = Interval(curInterval.start(), curInterval.finish()-1);
                Interval right = Interval(curInterval.start()+1, curInterval.finish());
                toSample.push_front(right);
                toSample.push_front(left);
            }
        }
    } while (!toSample.empty());

    return sampled;
}


#include "VDBVolume.h"

// OpenVDB
#include <openvdb/Types.h>
#include <openvdb/math/DDA.h>
#include <openvdb/math/Ray.h>
#include <openvdb/openvdb.h>

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <memory>
#include <vector>

namespace {

float ComputeSDF(const Eigen::Vector3d& origin,
                 const Eigen::Vector3d& point,
                 const Eigen::Vector3d& voxel_center) {
    const Eigen::Vector3d v_voxel_origin = voxel_center - origin;
    const Eigen::Vector3d v_point_voxel = point - voxel_center;
    const double dist = v_point_voxel.norm();
    const double proj = v_voxel_origin.dot(v_point_voxel);
    const double sign = proj / std::abs(proj);
    return static_cast<float>(sign * dist);
}

Eigen::Vector3d GetVoxelCenter(const openvdb::Coord& voxel, const openvdb::math::Transform& xform) {
    const float voxel_size = xform.voxelSize()[0];
    openvdb::math::Vec3d v_wf = xform.indexToWorld(voxel) + voxel_size / 2.0;
    return Eigen::Vector3d(v_wf.x(), v_wf.y(), v_wf.z());
}

}  // namespace

namespace vdbfusion {

VDBVolume::VDBVolume(float voxel_size, float sdf_trunc, bool space_carving /* = false*/)
    : voxel_size_(voxel_size), sdf_trunc_(sdf_trunc), space_carving_(space_carving) {
    tsdf_ = openvdb::FloatGrid::create(sdf_trunc_);
    tsdf_->setName("D(x): signed distance grid");
    tsdf_->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_));
    tsdf_->setGridClass(openvdb::GRID_LEVEL_SET);

    weights_ = openvdb::FloatGrid::create(0.0f);
    weights_->setName("W(x): weights grid");
    weights_->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_));
    weights_->setGridClass(openvdb::GRID_UNKNOWN);
}

void VDBVolume::UpdateTSDF(const float& sdf,
                           const openvdb::Coord& voxel,
                           const std::function<float(float)>& weighting_function) {
    using AccessorRW = openvdb::tree::ValueAccessorRW<openvdb::FloatTree>;
    if (sdf > -sdf_trunc_) {
        AccessorRW tsdf_acc = AccessorRW(tsdf_->tree());
        AccessorRW weights_acc = AccessorRW(weights_->tree());
        const float tsdf = std::min(sdf_trunc_, sdf);
        const float weight = weighting_function(sdf);
        const float last_weight = weights_acc.getValue(voxel);
        const float last_tsdf = tsdf_acc.getValue(voxel);
        const float new_weight = weight + last_weight;
        const float new_tsdf = (last_tsdf * last_weight + tsdf * weight) / (new_weight);
        tsdf_acc.setValue(voxel, new_tsdf);
        weights_acc.setValue(voxel, new_weight);
    }
}

void VDBVolume::Integrate(openvdb::FloatGrid::Ptr grid,
                          const std::function<float(float)>& weighting_function) {
    for (auto iter = grid->cbeginValueOn(); iter.test(); ++iter) {
        const auto& sdf = iter.getValue();
        const auto& voxel = iter.getCoord();
        this->UpdateTSDF(sdf, voxel, weighting_function);
    }
}

void VDBVolume::Integrate(const std::vector<Eigen::Vector3d>& points,
                          const Eigen::Vector3d& origin,
                          const std::function<float(float)>& weighting_function) {
    if (points.empty()) {
        std::cerr << "PointCloud provided is empty\n";
        return;
    }

    // Get some variables that are common to all rays
    const openvdb::math::Transform& xform = tsdf_->transform();
    const openvdb::Vec3R eye(origin.x(), origin.y(), origin.z());

    // Get the "unsafe" version of the grid acessors
    auto tsdf_acc = tsdf_->getUnsafeAccessor();
    auto weights_acc = weights_->getUnsafeAccessor();

    // Launch an for_each execution, use std::execution::par to parallelize this region
    std::for_each(points.cbegin(), points.cend(), [&](const auto& point) {
        // Get the direction from the sensor origin to the point and normalize it
        const Eigen::Vector3d direction = point - origin;
        openvdb::Vec3R dir(direction.x(), direction.y(), direction.z());
        dir.normalize();

        // Truncate the Ray before and after the source unless space_carving_ is specified.
        const auto depth = static_cast<float>(direction.norm());
        const float t0 = space_carving_ ? 0.0f : depth - sdf_trunc_;
        const float t1 = depth + sdf_trunc_;

        // Create one DDA per ray(per thread), the ray must operate on voxel grid coordinates.
        const auto ray = openvdb::math::Ray<float>(eye, dir, t0, t1).worldToIndex(*tsdf_);
        openvdb::math::DDA<decltype(ray)> dda(ray);
        do {
            const auto voxel = dda.voxel();
            const auto voxel_center = GetVoxelCenter(voxel, xform);
            const auto sdf = ComputeSDF(origin, point, voxel_center);
            if (sdf > -sdf_trunc_) {
                const float tsdf = std::min(sdf_trunc_, sdf);
                const float weight = weighting_function(sdf);
                const float last_weight = weights_acc.getValue(voxel);
                const float last_tsdf = tsdf_acc.getValue(voxel);
                const float new_weight = weight + last_weight;
                const float new_tsdf = (last_tsdf * last_weight + tsdf * weight) / (new_weight);
                tsdf_acc.setValue(voxel, new_tsdf);
                weights_acc.setValue(voxel, new_weight);
            }
        } while (dda.step());
    });
}

openvdb::FloatGrid::Ptr VDBVolume::Prune(float min_weight) const {
    const auto weights = weights_->tree();
    const auto tsdf = tsdf_->tree();
    const auto background = sdf_trunc_;
    openvdb::FloatGrid::Ptr clean_tsdf = openvdb::FloatGrid::create(sdf_trunc_);
    clean_tsdf->setName("D(x): Pruned signed distance grid");
    clean_tsdf->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_));
    clean_tsdf->setGridClass(openvdb::GRID_LEVEL_SET);
    clean_tsdf->tree().combine2Extended(tsdf, weights, [=](openvdb::CombineArgs<float>& args) {
        if (args.aIsActive() && args.b() > min_weight) {
            args.setResult(args.a());
            args.setResultIsActive(true);
        } else {
            args.setResult(background);
            args.setResultIsActive(false);
        }
    });
    return clean_tsdf;
}
}  // namespace vdbfusion


/**
 * @file max_coverage_example.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-03-13
 */
//! [Max Coverage Example]
#include <iostream>
#include <vector>
#include <iterator>

#include <boost/range/irange.hpp>

#include "paal/greedy/set_cover/maximum_coverage.hpp"

int main() {
    std::vector<std::vector<int>> set_to_elements = {
        { 1, 2 },
        { 3, 4, 5, 6 },
        { 7, 8, 9, 10, 11, 12, 13, 0 },
        { 1, 3, 5, 7, 9, 11, 13 },
        { 2, 4, 6, 8, 10, 12, 0 }
    };
    const int NUMBER_OF_SETS_TO_SELECT = 2;
    auto sets = boost::irange(0, 5);
    using SetIterator = decltype(sets)::iterator;
    std::vector<int> result;
    auto element_index = [](int el){return el;};
    auto covered = paal::greedy::maximum_coverage(
        sets,
        [&](int set){return set_to_elements[set];},
        back_inserter(result),
        element_index,
        NUMBER_OF_SETS_TO_SELECT);
    std::cout << "Covered: " << covered << std::endl;
}
//! [Max Coverage Example]


/*
 * This file is part of the Interpolated Polyline (https://github.com/fzi-forschungszentrum-informatik/P3IV),
 * copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
 */

#pragma once
#include <iostream>
#include <Eigen/Core>
#include "sequence_distribution.hpp"

namespace util_probability {

template <typename T>
struct TruncatedUnivariateNormalDistributionSequence : public UnivariateNormalDistributionSequence<T> {

    TruncatedUnivariateNormalDistributionSequence() = default;

    using Mean = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using Variance = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using Truncation = Eigen::Matrix<T, Eigen::Dynamic, 1>;

    // todo: check to pass (const std::vector<T> mean_ ...)
    TruncatedUnivariateNormalDistributionSequence(std::vector<T> mean_,
                                                  std::vector<T> variance_,
                                                  std::vector<T> upper_bound_,
                                                  std::vector<T> lower_bound_) {

        assert(mean_.size() == variance_.size());

        this->_mean = Eigen::Map<Mean>(mean_.data(), mean_.size(), 1);
        this->_covariance = Eigen::Map<Variance>(variance_.data(), variance_.size(), 1);
        this->_upper_truncation = Eigen::Map<Truncation>(upper_bound_.data(), upper_bound_.size(), 1);
        this->_lower_truncation = Eigen::Map<Truncation>(lower_bound_.data(), lower_bound_.size(), 1);
    }

    // todo: replace with range()
    std::vector<T> upperBound(const T sigma) const {

        std::vector<double> upper_bound;
        upper_bound.resize(this->_mean.size());

        for (size_t i = 0; i < upper_bound.size(); i++) {
            upper_bound[i] = std::min(_upper_truncation(i, 0), this->_mean(i, 0) + this->_covariance(i, 0) * sigma);
        }
        return upper_bound;
    }

    std::vector<T> lowerBound(const T sigma) const {

        std::vector<double> lower_bound;
        lower_bound.resize(this->_mean.size());

        for (size_t i = 0; i < lower_bound.size(); i++) {
            lower_bound[i] = std::max(_lower_truncation(i, 0), this->_mean(i, 0) - this->_covariance(i, 0) * sigma);
        }
        return lower_bound;
    }

protected:
    Truncation _upper_truncation;
    Truncation _lower_truncation;
};

} // namespace util_probability


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REMCEIL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REMCEIL_HPP_INCLUDED

#include <boost/simd/function/div.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
/////////////////////////////////////////////////////////////////////////////
// The remceil() function computes the remceil of dividing x by y.  The
// return value is x-n*y, where n is the value x / y, rounded toward +infinity
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::ceil_
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (bd::functor<bs::tag::ceil_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if (is_nez(a1))
        return fnms(div(ceil, a0, a1), a1, a0);
      else
        return a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::fast_tag
                          , bs::tag::ceil_
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &
                                    , bd::functor<bs::tag::ceil_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return fnms(div(ceil, a0, a1), a1, a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::fast_tag
                          , bs::tag::ceil_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &
                                    , bd::functor<bs::tag::ceil_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return  fnms(div(ceil, a0,a1), a1, a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::ceil_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::ceil_> const&
                                    , A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      if (is_nez(a1)&&is_eqz(a0)) return a0;
      return fnms(div(ceil, a0, a1), a1, a0);
    }
  };

} } }

#endif


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2009-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_LINALG_FUNCTIONS_MPOWER_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_FUNCTIONS_MPOWER_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/core/container/dsl/value_type.hpp>
#include <nt2/sdk/meta/value_as.hpp>
#include <nt2/core/utility/max_extent.hpp>
#include <nt2/sdk/meta/tieable_hierarchy.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <nt2/include/functions/isscalar.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/extent.hpp>
#include <boost/mpl/if.hpp>
#include <boost/assert.hpp>

namespace nt2
{
  namespace tag
  {
    /*!
     * \brief Define the tag mpower_ of functor mpower
     *        in namespace nt2::tag for toolbox algebra
     **/
    struct mpower_ :  ext::tieable_<mpower_>
    {
      typedef ext::tieable_<mpower_>  parent;
    };
  }
  /**
   * @brief compute matricial power a0^a1
   *
   * a0 or a1 can be a square matricial expression,  but one of the two must be scalar
   *
   * @param  a0  Matrix expression or scalar
   * @param  a1  Scalar or matrix expression
   *
   * @return a matrix containing a0^a1
   **/

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mpower_, mpower, 2)

}

namespace nt2 { namespace ext
{
  template<class Domain, int N, class Expr>
  struct  size_of<tag::mpower_,Domain,N,Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&,0>::value_type  c0_t;
    typedef typename boost::proto::result_of::child_c<Expr&,1>::value_type  c1_t;
    typedef typename c0_t::extent_type                                     ex0_t;
    typedef typename c1_t::extent_type                                     ex1_t;
    typedef typename utility::result_of::max_extent<ex0_t, ex1_t>::type         result_type;
    BOOST_FORCEINLINE result_type operator()(Expr& e) const
    {
      BOOST_ASSERT_MSG((isscalar( boost::proto::child_c<0>(e))&&issquare(boost::proto::child_c<1>(e)))||
                       (isscalar( boost::proto::child_c<1>(e))&&issquare(boost::proto::child_c<0>(e))),
                       "mpower needs a square matrix expression and a scalar or a scalar and a square matrix expression");

      return nt2::utility::max_extent(nt2::extent(boost::proto::child_c<0>(e)),  nt2::extent(boost::proto::child_c<1>(e)));
    }
  };

  template<class Domain, int N, class Expr>
  struct  value_type<tag::mpower_,Domain,N,Expr>
        : meta::value_as<Expr,0>
  {
    typedef typename  boost::proto::result_of::child_c<Expr&,0>::value_type::value_type v0_t;
    typedef typename  boost::proto::result_of::child_c<Expr&,1>::value_type::value_type v1_t;
    typedef typename  meta::is_complex<v0_t>::type                                  iscplx_0;
    typedef typename  meta::is_complex<v1_t>::type                                  iscplx_1;
    typedef typename  boost::mpl::if_<iscplx_0, v0_t, v1_t>::type                       t0_t;
    typedef typename  boost::mpl::if_<iscplx_1, v1_t, t0_t>::type                       type;
  };
} }

#endif


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_NV_HPP
#define ODE_NV_HPP

// ODE (not verified)

#include <iostream>
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/psa.hpp>
#include <kv/ode-param.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

namespace kv{

namespace ub = boost::numeric::ublas;

template <class T, class F>
void
ode_nv(F f, ub::vector<T>& init, const T& start, T& end, ode_param<T> p = ode_param<T>()) {
	int n = init.size();
	int i, j;

	ub::vector< psa<T> > x, y;
	psa<T> torg;
	psa<T> t;

	T deltat;
	ub::vector<T> result;

	T m;

	T radius, radius_tmp;
	T tolerance;
	int n_rad;

	bool save_mode, save_uh, save_rh;

	m = 1.;
	for (i=0; i<n; i++) {
		using std::abs;
		m = std::max(m, abs(init(i)));
	}
	tolerance = m * p.epsilon;

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa<T>::mode();
	save_uh = psa<T>::use_history();
	save_rh = psa<T>::record_history();
	psa<T>::mode() = 1;
	psa<T>::use_history() = false;
	psa<T>::record_history() = false;
	#if ODE_FAST == 1
	psa<T>::record_history() = true;
	psa<T>::history().clear();
	#endif
	for (j=0; j<p.order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa<T>::use_history() = true;
		if (j == p.order - 1) psa<T>::record_history() = false;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) {
			y(i) = integrate(y(i));
			y(i) = setorder(y(i), j+1);
		}
		x = init + y;
	}

	if (p.autostep) {
		radius = 0.;
		n_rad = 0;
		for (j = p.order; j>=1; j--) {
			m = 0.;
			for (i=0; i<n; i++) {
				using std::abs;
				m = std::max(m, abs(x(i).v(j)));
			}
			if (m == 0.) continue;
			radius_tmp = std::pow((double)m, 1./j);
			if (radius_tmp > radius) radius = radius_tmp;
			n_rad++;
			if (n_rad == 2) break;
		}
		radius = std::pow((double)tolerance, 1./p.order) / radius;
	}

	deltat = end - start;

	if (p.autostep && radius < deltat) {
		end = start + radius;
		deltat = end - start;
	}

	result.resize(n);
	for (i=0; i<n; i++) {
		result(i) = eval(x(i), deltat);
	}

	init = result;

	psa<T>::mode() = save_mode;
	psa<T>::use_history() = save_uh;
	psa<T>::record_history() = save_rh;
}

template <class T, class F>
void
odelong_nv(F f, ub::vector<T>& init, const T& start, const T& end, ode_param<T> p = ode_param<T>()) {

	ub::vector<T> x;
	T t, t1;

	x = init;
	t = start;
	p.set_autostep(true);
	while (1) {
		t1 = end;
		if (t == t1) break;

		ode_nv(f, x, t, t1, p);
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << x << "\n";
		}
		t = t1;
	}

	init = x;
}

} // namespace kv

#endif // ODE_NV_HPP


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// This file was originally part of PyJuggler.

// PyJuggler is (C) Copyright 2002, 2003 by Patrick Hartling
// Distributed under the GNU Lesser General Public License 2.1.  (See
// accompanying file COPYING.txt or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl-wrappers.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================

namespace  {

BOOST_PYTHON_FUNCTION_OVERLOADS(makeDirCosMatrix33_overloads_3_6, gmtlWrappers::makeDirCosMatrix33, 3, 6)
BOOST_PYTHON_FUNCTION_OVERLOADS(makeDirCosMatrix44_overloads_3_6, gmtlWrappers::makeDirCosMatrix44, 3, 6)

}// namespace

// Module ======================================================================
void _Export_gmtl_wrappers_h()
{
    def("makeAxesMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &))&gmtlWrappers::makeAxesMatrix33);
    def("makeAxesMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtlWrappers::makeAxesMatrix33);
    def("makeAxesMatrix44", (gmtl::Matrix<double,4,4> (*)(const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &))&gmtlWrappers::makeAxesMatrix44);
    def("makeAxesMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtlWrappers::makeAxesMatrix44);
    def("makeDirCosMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &))&gmtlWrappers::makeDirCosMatrix33, makeDirCosMatrix33_overloads_3_6());
    def("makeDirCosMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtlWrappers::makeDirCosMatrix33, makeDirCosMatrix33_overloads_3_6());
    def("makeDirCosMatrix44", (gmtl::Matrix<double,4,4> (*)(const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &))&gmtlWrappers::makeDirCosMatrix44, makeDirCosMatrix44_overloads_3_6());
    def("makeDirCosMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtlWrappers::makeDirCosMatrix44, makeDirCosMatrix44_overloads_3_6());
    def("makeRotEulerAngleXYZ", (gmtl::EulerAngle<float,gmtl::XYZ> (*)(const gmtl::Matrix<float,4,4> &))&gmtlWrappers::makeRotEulerAngleXYZ);
    def("makeRotEulerAngleXYZ", (gmtl::EulerAngle<double,gmtl::XYZ> (*)(const gmtl::Matrix<double,4,4> &))&gmtlWrappers::makeRotEulerAngleXYZ);
    def("makeRotEulerAngleZXY", (gmtl::EulerAngle<float,gmtl::ZXY> (*)(const gmtl::Matrix<float,4,4> &))&gmtlWrappers::makeRotEulerAngleZXY);
    def("makeRotEulerAngleZXY", (gmtl::EulerAngle<double,gmtl::ZXY> (*)(const gmtl::Matrix<double,4,4> &))&gmtlWrappers::makeRotEulerAngleZXY);
    def("makeRotEulerAngleZYX", (gmtl::EulerAngle<float,gmtl::ZYX> (*)(const gmtl::Matrix<float,4,4> &))&gmtlWrappers::makeRotEulerAngleZYX);
    def("makeRotEulerAngleZYX", (gmtl::EulerAngle<double,gmtl::ZYX> (*)(const gmtl::Matrix<double,4,4> &))&gmtlWrappers::makeRotEulerAngleZYX);
    def("makeRotMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::Quat<double> &))&gmtlWrappers::makeRotMatrix33);
    def("makeRotMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::Quat<float> &))&gmtlWrappers::makeRotMatrix33);
    def("makeRotMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::EulerAngle<float,gmtl::ZYX> &))&gmtlWrappers::makeRotMatrix33);
    def("makeRotMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::EulerAngle<float,gmtl::XYZ> &))&gmtlWrappers::makeRotMatrix33);
    def("makeRotMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::EulerAngle<float,gmtl::ZXY> &))&gmtlWrappers::makeRotMatrix33);
    def("makeRotMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::AxisAngle<float> &))&gmtlWrappers::makeRotMatrix33);
    def("makeRotMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::AxisAngle<double> &))&gmtlWrappers::makeRotMatrix33);
    def("makeRotMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::Quat<float> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::EulerAngle<float,gmtl::XYZ> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::EulerAngle<float,gmtl::ZXY> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotMatrix44", (gmtl::Matrix<double,4,4> (*)(const gmtl::Quat<double> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::EulerAngle<float,gmtl::ZYX> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::AxisAngle<float> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotMatrix44", (gmtl::Matrix<double,4,4> (*)(const gmtl::AxisAngle<double> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtlWrappers::makeRotMatrix44);
    def("makeRotQuat", (gmtl::Quat<float> (*)(const gmtl::Matrix<float,4,4> &))&gmtlWrappers::makeRotQuat);
    def("makeRotQuat", (gmtl::Quat<double> (*)(const gmtl::Matrix<double,4,4> &))&gmtlWrappers::makeRotQuat);
    def("makeRotQuat", (gmtl::Quat<float> (*)(const gmtl::AxisAngle<float> &))&gmtlWrappers::makeRotQuat);
    def("makeRotQuat", (gmtl::Quat<double> (*)(const gmtl::AxisAngle<double> &))&gmtlWrappers::makeRotQuat);
    def("makeRotQuat", (gmtl::Quat<float> (*)(const gmtl::Vec<float,3> &, const gmtl::Vec<float,3> &))&gmtlWrappers::makeRotQuat);
    def("makeRotQuat", (gmtl::Quat<double> (*)(const gmtl::Vec<double,3> &, const gmtl::Vec<double,3> &))&gmtlWrappers::makeRotQuat);
    def("makeScaleMatrix33", (gmtl::Matrix<double,3,3> (*)(const double&))&gmtlWrappers::makeScaleMatrix33);
    def("makeScaleMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::Vec<double,2> &))&gmtlWrappers::makeScaleMatrix33);
    def("makeScaleMatrix33", (gmtl::Matrix<float,3,3> (*)(const float&))&gmtlWrappers::makeScaleMatrix33);
    def("makeScaleMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::Vec<double,3> &))&gmtlWrappers::makeScaleMatrix33);
    def("makeScaleMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::Vec<float,2> &))&gmtlWrappers::makeScaleMatrix33);
    def("makeScaleMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::Vec<float,3> &))&gmtlWrappers::makeScaleMatrix33);
    def("makeScaleMatrix44", (gmtl::Matrix<double,4,4> (*)(const double&))&gmtlWrappers::makeScaleMatrix44);
    def("makeScaleMatrix44", (gmtl::Matrix<double,4,4> (*)(const gmtl::Vec<double,4> &))&gmtlWrappers::makeScaleMatrix44);
    def("makeScaleMatrix44", (gmtl::Matrix<float,4,4> (*)(const float&))&gmtlWrappers::makeScaleMatrix44);
    def("makeScaleMatrix44", (gmtl::Matrix<double,4,4> (*)(const gmtl::Vec<double,3> &))&gmtlWrappers::makeScaleMatrix44);
    def("makeScaleMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::Vec<float,4> &))&gmtlWrappers::makeScaleMatrix44);
    def("makeScaleMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::Vec<float,3> &))&gmtlWrappers::makeScaleMatrix44);
    def("makeTransMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::Vec<double,2> &))&gmtlWrappers::makeTransMatrix33);
    def("makeTransMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::Vec<float,2> &))&gmtlWrappers::makeTransMatrix33);
    def("makeTransMatrix33", (gmtl::Matrix<double,3,3> (*)(const gmtl::Vec<double,3> &))&gmtlWrappers::makeTransMatrix33);
    def("makeTransMatrix33", (gmtl::Matrix<float,3,3> (*)(const gmtl::Vec<float,3> &))&gmtlWrappers::makeTransMatrix33);
    def("makeTransMatrix44", (gmtl::Matrix<double,4,4> (*)(const gmtl::Vec<double,3> &))&gmtlWrappers::makeTransMatrix44);
    def("makeTransMatrix44", (gmtl::Matrix<float,4,4> (*)(const gmtl::Vec<float,3> &))&gmtlWrappers::makeTransMatrix44);
    def("makeTransVec3", (gmtl::Vec<double,3> (*)(const gmtl::Matrix<double,4,4> &))&gmtlWrappers::makeTransVec3);
    def("makeTransVec3", (gmtl::Vec<float,3> (*)(const gmtl::Matrix<float,4,4> &))&gmtlWrappers::makeTransVec3);
    def("makeTransVec3", (gmtl::Vec<double,3> (*)(const gmtl::Matrix<double,3,3> &))&gmtlWrappers::makeTransVec3);
    def("makeTransVec3", (gmtl::Vec<float,3> (*)(const gmtl::Matrix<float,3,3> &))&gmtlWrappers::makeTransVec3);
    def("makeTransVec2", (gmtl::Vec<double,2> (*)(const gmtl::Matrix<double,3,3> &))&gmtlWrappers::makeTransVec2);
    def("makeTransVec2", (gmtl::Vec<float,2> (*)(const gmtl::Matrix<float,3,3> &))&gmtlWrappers::makeTransVec2);
    def("makeTransPoint3", (gmtl::Point<double,3> (*)(const gmtl::Matrix<double,4,4> &))&gmtlWrappers::makeTransPoint3);
    def("makeTransPoint3", (gmtl::Point<float,3> (*)(const gmtl::Matrix<float,4,4> &))&gmtlWrappers::makeTransPoint3);
}