/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis
 * Copyright (c) Leipzig, Madrid 1999-2017 Gert Wollny
 *
 * MIA is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIA; if not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <mia/mesh/clist.hh>
#include <mia/core/msgstream.hh>
#include <iostream>

NS_MIA_BEGIN

/**
   \brief class to make a triangle mesh from a closed polygon

   This class is used to convert a closed polygon into a mesh defined
   by triangles.

   \tparam VertexVector type of the vector that contains the vertices
   \tparam Polygon The type of the Polygon

*/
template <class VertexVector, class Polygon>
class TPolyTriangulator
{
public:
       /**
          Constructor to be initializes with a vector of vertices that used when defining the polygon
          \param vv the vertices
        */
       TPolyTriangulator(const VertexVector& vv);

       /**
          Evaluate the triangulation of the polygon that is defined by the vertices given above and
          their ordering by indices as given in poly.
          \param [out] output the list of triangles as list of index triples into the vertex arrey given in the
          constructor
          \param [in] poly the index array into the vertex array describing the polygon
          \returns true of the triangulation was successfull and false if the polygon only consisted only
          of two corners.
        */
       template <class TriangleList>
       bool triangulate(TriangleList& output, const Polygon& poly) const;

private:
       typedef clist<typename Polygon::value_type> CPoly;
       typedef typename VertexVector::value_type Vector;

       Vector eval_orientation(const Polygon& poly) const;
       bool is_convex(const typename CPoly::const_iterator& i, bool debug = false) const;
       bool is_ear(const typename CPoly::const_iterator& p, const CPoly& cpoly, bool debug = false) const;

       bool is_inside(
              const typename VertexVector::value_type& a,
              const typename VertexVector::value_type& b,
              const typename VertexVector::value_type& c,
              const typename VertexVector::value_type& p,
              bool debug = false ) const;


       const VertexVector& m_vv;
       mutable Vector m_orientation;
};

template <class VertexVector, class Polygon>
TPolyTriangulator<VertexVector, Polygon>::TPolyTriangulator(const VertexVector& vv):
       m_vv(vv)
{
}

template <class VertexVector, class Polygon>
template <class TriangleList>
bool
TPolyTriangulator<VertexVector, Polygon>::triangulate(TriangleList& output, const Polygon& poly) const
{
       size_t poly_size = poly.size();

       if ( poly_size < 3) // no triangles at all
              return false;

       typedef clist<typename Polygon::value_type> CPoly;
       CPoly cpoly;
       m_orientation = eval_orientation(poly);
       typename Polygon::const_iterator pi = poly.begin();
       typename Polygon::const_iterator pe = poly.end();

       while (pi != pe)
              cpoly.push_back(*pi++);

       typename CPoly::iterator  p_i = cpoly.begin();
       typename CPoly::iterator  p_e = cpoly.end();
       p_i = p_i->succ;

       while ( (p_i != p_e) && (poly_size > 3)) {
              if (is_ear(p_i, cpoly, false)) {
                     // we have a valid triangle, store it
                     output.push_back(typename TriangleList::value_type(**p_i->succ, **p_i, **p_i->prev));
                     // set the current middle node
                     typename CPoly::iterator  p_r = p_i;
                     p_i = (p_i->prev != cpoly.begin()) ? p_i->prev : p_i->succ;
                     cpoly.remove(p_r);
                     --poly_size;
              } else
                     p_i = p_i->succ;
       }

       if ((p_i == p_e) && (poly_size > 3)) {
              cvdebug()  << "gotcha\n";
              p_i = cpoly.begin();
              p_i = p_i->succ;

              while ( (p_i != p_e) && (poly_size > 3)) {
                     if (is_ear(p_i, cpoly, true)) {
                            // we have a valid triangle, store it
                            output.push_back(typename TriangleList::value_type(**p_i->succ, **p_i, **p_i->prev));
                            // set the current middle node
                            typename CPoly::iterator  p_r = p_i;
                            p_i = (p_i->prev != cpoly.begin()) ? p_i->prev : p_i->succ;
                            cpoly.remove(p_r);
                            --poly_size;
                     } else
                            p_i = p_i->succ;
              }
       }

       output.push_back(typename TriangleList::value_type(**p_i->succ, **p_i, **p_i->prev));
       return true;
}



template <class VertexVector, class Polygon>
typename TPolyTriangulator<VertexVector, Polygon>::Vector
TPolyTriangulator<VertexVector, Polygon>::eval_orientation(const Polygon& poly) const
{
       typename VertexVector::value_type result(0, 0, 0);
       typename Polygon::const_iterator pb = poly.begin();
       typename Polygon::const_iterator be = poly.end();
       typename Polygon::const_iterator c1 = pb;
       ++c1;
       typename Polygon::const_iterator c2 = c1;
       ++c2;
       typename VertexVector::value_type a = m_vv[*pb];

       while (c1 != be && c2 != be) {
              result += (m_vv[*c1++] - a) ^ (m_vv[*c2++] - a);
       }

       return result;
}

template <class VertexVector, class Polygon>
bool TPolyTriangulator<VertexVector, Polygon>::is_convex(const typename CPoly::const_iterator& i, bool /*debug*/) const
{
       typename VertexVector::value_type a = m_vv[ **i->prev];
       typename VertexVector::value_type b = m_vv[ **i];
       typename VertexVector::value_type c = m_vv[ **i->succ];
       typename VertexVector::value_type ab = a - b;
       typename VertexVector::value_type cb = c - b;
       typename VertexVector::value_type cross = ab ^ cb;
       const bool result = dot(cross, m_orientation) > 0;
       return result;
}

template <class VertexVector, class Polygon>
bool TPolyTriangulator<VertexVector, Polygon>::is_ear(const typename CPoly::const_iterator& p, const CPoly& cpoly, bool debug) const
{
       if (!is_convex(p, debug)) {
              cvdebug() << "corner is concave\n";
              return false;
       }

       typename VertexVector::value_type a = m_vv[ **p->prev];
       typename VertexVector::value_type b = m_vv[ **p];
       typename VertexVector::value_type c = m_vv[ **p->succ];
       cvdebug() << "check triangle" << a << b << c << " = (" << **p->prev << "," << **p << "," << **p->succ << "\n";
       typename CPoly::const_iterator i = cpoly.begin();
       i = i->succ;

       while (i != cpoly.end()) {
              if (i != p && i != p->prev && i != p->succ)
                     if (!is_convex(i, debug) && is_inside(a, b, c, m_vv[ **i], debug)) {
                            cvdebug() << "point " << **i << ":" << m_vv[ **i] << " is concave and inside\n";
                            return false;
                     }

              i = i->succ;
       }

       return true;
}

template <class VertexVector, class Polygon>
bool TPolyTriangulator<VertexVector, Polygon>::is_inside(
       const typename VertexVector::value_type& a,
       const typename VertexVector::value_type& b,
       const typename VertexVector::value_type& c,
       const typename VertexVector::value_type& p,
       bool /*debug*/) const
{
       double abc = ((a - b) ^ (c - b)).norm() * 0.5;
       double abp = ((a - p) ^ (b - p)).norm() * 0.5;
       double acp = ((a - p) ^ (c - p)).norm() * 0.5;
       double bcp = ((b - p) ^ (c - p)).norm() * 0.5;
       const bool result = (abc >= abp + acp + bcp);
       return result;
}

NS_MIA_END
