/* GTS - Library for the manipulation of triangulated surfaces
 * Copyright (C) 1999-2002 Ray Jones, Stéphane Popinet
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

#include <cstdlib> // for malloc and free
#include "Curvature.h"
#include <math.h>
#include <assert.h>
#include "WEdge.h"
#include "../system/FreestyleConfig.h"
#include "../geometry/normal_cycle.h"
#include <set>
#include <stack>

static bool angle_obtuse (WVertex * v, WFace * f)
{
  WOEdge * e; 
  f->getOppositeEdge (v, e);
  
  Vec3r vec1(e->GetaVertex()->GetVertex()-v->GetVertex());
  Vec3r vec2(e->GetbVertex()->GetVertex()-v->GetVertex());
  return ((vec1 * vec2) < 0);
}

// FIXME
// WVvertex is useless but kept for history reasons
static bool triangle_obtuse (WVertex*, WFace * f)
{
  bool b=false;
  for (int i=0; i<3; i++)
    b = b ||
      ((f->getEdgeList()[i]->GetVec() * f->getEdgeList()[(i+1)%3]->GetVec()) < 0);
  return b;
}

static real cotan (WVertex * vo, WVertex * v1, WVertex * v2)
{
  /* cf. Appendix B of [Meyer et al 2002] */
  real udotv, denom;

  Vec3r u(v1->GetVertex()- vo->GetVertex());
  Vec3r v(v2->GetVertex()- vo->GetVertex());

  udotv = u * v;
  denom = sqrt(u.squareNorm() * v.squareNorm() - udotv * udotv);

  /* denom can be zero if u==v.  Returning 0 is acceptable, based on
   * the callers of this function below. */
  if (denom == 0.0) return (0.0);

  return (udotv / denom);
}

static real angle_from_cotan (WVertex * vo, WVertex * v1, WVertex * v2)
{
  /* cf. Appendix B and the caption of Table 1 from [Meyer et al 2002] */
  real udotv, denom;

  Vec3r u (v1->GetVertex()-vo->GetVertex());
  Vec3r v(v2->GetVertex()-vo->GetVertex());

  udotv = u * v;
  denom = sqrt(u.squareNorm() * v.squareNorm() - udotv * udotv);

  /* Note: I assume this is what they mean by using atan2 (). -Ray Jones */

  /* tan = denom/udotv = y/x (see man page for atan2) */
  return (fabs (atan2 (denom, udotv)));
}

/** 
 * gts_vertex_mean_curvature_normal:
 * @v: a #WVertex.  
 * @s: a #GtsSurface.
 * @Kh: the Mean Curvature Normal at @v.
 *
 * Computes the Discrete Mean Curvature Normal approximation at @v.
 * The mean curvature at @v is half the magnitude of the vector @Kh.
 *
 * Note: the normal computed is not unit length, and may point either
 * into or out of the surface, depending on the curvature at @v.  It
 * is the responsibility of the caller of the function to use the mean
 * curvature normal appropriately.
 *
 * This approximation is from the paper:
 * Discrete Differential-Geometry Operators for Triangulated 2-Manifolds
 * Mark Meyer, Mathieu Desbrun, Peter Schroder, Alan H. Barr
 * VisMath '02, Berlin (Germany) 
 * http://www-grail.usc.edu/pubs.html
 *
 * Returns: %TRUE if the operator could be evaluated, %FALSE if the
 * evaluation failed for some reason (@v is boundary or is the
 * endpoint of a non-manifold edge.)
 */
bool gts_vertex_mean_curvature_normal (WVertex * v, Vec3r &Kh)
{
	real area = 0.0;

	if (!v) return false;

	/* this operator is not defined for boundary edges */
	if (v->isBoundary()) return false;
    
	WVertex::incoming_edge_iterator itE;

	for (itE=v->incoming_edges_begin();
	     itE!=v->incoming_edges_end(); itE++)
			 area+=(*itE)->GetaFace()->getArea();

	Kh=Vec3r(0.0, 0.0, 0.0);

	for (itE=v->incoming_edges_begin();
	     itE!=v->incoming_edges_end(); itE++)
		{
			WOEdge * e = (*itE)->getPrevOnFace();
			//if ((e->GetaVertex()==v) || (e->GetbVertex()==v)) cerr<< "BUG ";

			WVertex * v1 = e->GetaVertex();
			WVertex * v2 = e->GetbVertex();
			real temp;

			temp = cotan (v1, v, v2);
			Kh = Vec3r(Kh+temp*(v2->GetVertex()-v->GetVertex()));

			temp = cotan (v2, v, v1);
			Kh = Vec3r(Kh+temp*(v1->GetVertex()-v->GetVertex()));
		}
	if (area > 0.0) 
	{
		Kh[0] /= 2*area;
		Kh[1] /= 2*area;
		Kh[2] /= 2*area;
	} 
	else return false;

	return true;
}

/** 
 * gts_vertex_gaussian_curvature:
 * @v: a #WVertex.  
 * @s: a #GtsSurface.
 * @Kg: the Discrete Gaussian Curvature approximation at @v.
 *
 * Computes the Discrete Gaussian Curvature approximation at @v.
 *
 * This approximation is from the paper:
 * Discrete Differential-Geometry Operators for Triangulated 2-Manifolds
 * Mark Meyer, Mathieu Desbrun, Peter Schroder, Alan H. Barr
 * VisMath '02, Berlin (Germany) 
 * http://www-grail.usc.edu/pubs.html
 *
 * Returns: %TRUE if the operator could be evaluated, %FALSE if the
 * evaluation failed for some reason (@v is boundary or is the
 * endpoint of a non-manifold edge.)
 */
bool gts_vertex_gaussian_curvature (WVertex * v,  real * Kg)
{
	real area = 0.0;
	real angle_sum = 0.0;

	if (!v) return false; 
	if (!Kg) return false; 

 	/* this operator is not defined for boundary edges */
	if (v->isBoundary()) {*Kg=0.0 ;return false;}
    
	WVertex::incoming_edge_iterator itE;
	for (itE=v->incoming_edges_begin();
	     itE!=v->incoming_edges_end(); itE++)
			 area+=(*itE)->GetaFace()->getArea();

 	for (itE=v->incoming_edges_begin();
	     itE!=v->incoming_edges_end(); itE++)
		{
			WOEdge * e = (*itE)->getPrevOnFace();
			WVertex * v1 = e->GetaVertex();
			WVertex * v2 = e->GetbVertex(); 
			angle_sum += angle_from_cotan (v, v1, v2);
		}
 
	*Kg = (2.0*M_PI - angle_sum)/area;

	return true;
}

/** 
 * gts_vertex_principal_curvatures:
 * @Kh: mean curvature.
 * @Kg: Gaussian curvature.
 * @K1: first principal curvature.
 * @K2: second principal curvature.
 *
 * Computes the principal curvatures at a point given the mean and
 * Gaussian curvatures at that point.  
 *
 * The mean curvature can be computed as one-half the magnitude of the
 * vector computed by gts_vertex_mean_curvature_normal().
 *
 * The Gaussian curvature can be computed with
 * gts_vertex_gaussian_curvature().
 */
void gts_vertex_principal_curvatures (real Kh, real Kg, 
				      real * K1, real * K2)
{
  real temp = Kh*Kh - Kg;

  if (!K1) return; 
  if (!K1) return; 

  if (temp < 0.0) temp = 0.0;
  temp = sqrt (temp);
  *K1 = Kh + temp;
  *K2 = Kh - temp;
}

/* from Maple */
static void linsolve (real m11, real m12, real b1,
		      real m21, real m22, real b2,
		      real * x1, real * x2)
{
  real temp;

  temp = 1.0 / (m21*m12 - m11*m22);
  *x1 = (m12*b2 - m22*b1)*temp;
  *x2 = (m11*b2 - m21*b1)*temp;
}
                
/* from Maple - largest eigenvector of [a b; b c] */
static void eigenvector (real a, real b, real c,
			 Vec3r e)
{
  if (b == 0.0) {
    e[0] = 0.0;
  } else {
    e[0] = -(c - a - sqrt (c*c - 2*a*c + a*a + 4*b*b))/(2*b);
  }
  e[1] = 1.0;
  e[2] = 0.0;
}

/** 
 * gts_vertex_principal_directions:
 * @v: a #WVertex.  
 * @s: a #GtsSurface.
 * @Kh: mean curvature normal (a #Vec3r).
 * @Kg: Gaussian curvature (a real).
 * @e1: first principal curvature direction (direction of largest curvature).
 * @e2: second principal curvature direction.
 *
 * Computes the principal curvature directions at a point given @Kh
 * and @Kg, the mean curvature normal and Gaussian curvatures at that
 * point, computed with gts_vertex_mean_curvature_normal() and
 * gts_vertex_gaussian_curvature(), respectively. 
 *
 * Note that this computation is very approximate and tends to be
 * unstable.  Smoothing of the surface or the principal directions may
 * be necessary to achieve reasonable results.  
 */
void gts_vertex_principal_directions (WVertex * v, 
                                      Vec3r Kh, real Kg,
									  Vec3r &e1, Vec3r &e2)
{
  Vec3r N;
  real normKh;

  Vec3r basis1, basis2, d, eig;
  real ve2, vdotN;
  real aterm_da, bterm_da, cterm_da, const_da;
  real aterm_db, bterm_db, cterm_db, const_db;
  real a, b, c;
  real K1, K2;
  real *weights, *kappas, *d1s, *d2s;
  int edge_count;
  real err_e1, err_e2;
  int e;
  WVertex::incoming_edge_iterator itE;

  /* compute unit normal */
  normKh = Kh.norm();

  if (normKh > 0.0) {
    Kh.normalize();
  } else {
    /* This vertex is a point of zero mean curvature (flat or saddle
     * point).  Compute a normal by averaging the adjacent triangles
     */
    N[0] = N[1] = N[2] = 0.0;

    for (itE=v->incoming_edges_begin();
	 itE!=v->incoming_edges_end(); itE++)
      N=Vec3r(N+(*itE)->GetaFace()->GetNormal());
    real normN = N.norm();
    if (normN <= 0.0)
      return;
    N.normalize();
  }
    

  /* construct a basis from N: */
  /* set basis1 to any component not the largest of N */
  basis1[0] =  basis1[1] =  basis1[2] = 0.0;
  if (fabs (N[0]) > fabs (N[1]))
    basis1[1] = 1.0;
  else
    basis1[0] = 1.0;
    
  /* make basis2 orthogonal to N */
  basis2 = (N ^ basis1);
  basis2.normalize();

  /* make basis1 orthogonal to N and basis2 */
  basis1 = (N ^ basis2);
  basis1.normalize();
  
  aterm_da = bterm_da = cterm_da = const_da = 0.0;
  aterm_db = bterm_db = cterm_db = const_db = 0.0;
  int nb_edges=v->GetEdges().size();

  weights = (real *) malloc (sizeof (real)*nb_edges);
  kappas = (real*) malloc (sizeof (real)*nb_edges);
  d1s = (real*) malloc (sizeof (real)*nb_edges);
  d2s = (real*) malloc (sizeof (real)*nb_edges);
  edge_count = 0;

  for (itE=v->incoming_edges_begin();
	   itE!=v->incoming_edges_end(); itE++)
	   {
			WOEdge * e;
			WFace * f1, * f2;
			real weight, kappa, d1, d2;
			Vec3r vec_edge;
			if (! *itE) continue;
			e = *itE;

			/* since this vertex passed the tests in
			* gts_vertex_mean_curvature_normal(), this should be true. */
			//g_assert (gts_edge_face_number (e, s) == 2);

			/* identify the two triangles bordering e in s */
			f1=e->GetaFace();
			f2=e->GetbFace();

    /* We are solving for the values of the curvature tensor 
     *     B = [ a b ; b c ].  
     * The computations here are from section 5 of [Meyer et al 2002].  
     *
     * The first step is to calculate the linear equations governing
     * the values of (a,b,c).  These can be computed by setting the
     * derivatives of the error E to zero (section 5.3).
     * 
     * Since a + c = norm(Kh), we only compute the linear equations
     * for dE/da and dE/db.  (NB: [Meyer et al 2002] has the
     * equation a + b = norm(Kh), but I'm almost positive this is
     * incorrect.)
     *
     * Note that the w_ij (defined in section 5.2) are all scaled by
     * (1/8*A_mixed).  We drop this uniform scale factor because the
     * solution of the linear equations doesn't rely on it.
     *
     * The terms of the linear equations are xterm_dy with x in
     * {a,b,c} and y in {a,b}.  There are also const_dy terms that are
     * the constant factors in the equations.  
     */

    /* find the vector from v along edge e */
    vec_edge=Vec3r(-1*e->GetVec());

    ve2 = vec_edge.squareNorm();
    vdotN = vec_edge * N;

    /* section 5.2 - There is a typo in the computation of kappa.  The
     * edges should be x_j-x_i.
     */
    kappa = 2.0 * vdotN / ve2;

    /* section 5.2 */

    /* I don't like performing a minimization where some of the
     * weights can be negative (as can be the case if f1 or f2 are
     * obtuse).  To ensure all-positive weights, we check for
     * obtuseness. */
    weight = 0.0;
    if (! triangle_obtuse(v, f1)) {
      weight += ve2 * 
        cotan (f1->GetNextOEdge(e->twin())->GetbVertex(), 
               e->GetaVertex(), e->GetbVertex()) / 8.0;
    } else {
      if (angle_obtuse (v, f1)) {
        weight += ve2 * f1->getArea() / 4.0;
      } else {
        weight += ve2 * f1->getArea() / 8.0;
      }
    }

    if (! triangle_obtuse(v, f2)) {
      weight += ve2 * 
        cotan (f2->GetNextOEdge(e)->GetbVertex(), 
              e->GetaVertex(), e->GetbVertex()) / 8.0;
    } else {
      if (angle_obtuse (v, f2)) {
        weight += ve2 * f1->getArea() / 4.0;
      } else {
        weight += ve2 * f1->getArea()  / 8.0;
      }
    }

    /* projection of edge perpendicular to N (section 5.3) */
    d[0] = vec_edge[0] - vdotN * N[0];
    d[1] = vec_edge[1] - vdotN * N[1];
    d[2] = vec_edge[2] - vdotN * N[2];
    d.normalize();
    
    /* not explicit in the paper, but necessary.  Move d to 2D basis. */
    d1 = d * basis1;
    d2 = d * basis2;

    /* store off the curvature, direction of edge, and weights for later use */
    weights[edge_count] = weight;
    kappas[edge_count] = kappa;
    d1s[edge_count] = d1;
    d2s[edge_count] = d2;
    edge_count++;

    /* Finally, update the linear equations */
    aterm_da += weight * d1 * d1 * d1 * d1;
    bterm_da += weight * d1 * d1 * 2 * d1 * d2;
    cterm_da += weight * d1 * d1 * d2 * d2;
    const_da += weight * d1 * d1 * (- kappa);

    aterm_db += weight * d1 * d2 * d1 * d1;
    bterm_db += weight * d1 * d2 * 2 * d1 * d2;
    cterm_db += weight * d1 * d2 * d2 * d2;
    const_db += weight * d1 * d2 * (- kappa);

   }

  /* now use the identity (Section 5.3) a + c = |Kh| = 2 * kappa_h */
  aterm_da -= cterm_da;
  const_da += cterm_da * normKh;

  aterm_db -= cterm_db;
  const_db += cterm_db * normKh;
  
  /* check for solvability of the linear system */
  if (((aterm_da * bterm_db - aterm_db * bterm_da) != 0.0) &&
      ((const_da != 0.0) || (const_db != 0.0))) {
    linsolve (aterm_da, bterm_da, -const_da,
              aterm_db, bterm_db, -const_db,
              &a, &b);

    c = normKh - a;

    eigenvector (a, b, c, eig);
  } else {
    /* region of v is planar */
    eig[0] = 1.0;
    eig[1] = 0.0;
  }

  /* Although the eigenvectors of B are good estimates of the
   * principal directions, it seems that which one is attached to
   * which curvature direction is a bit arbitrary.  This may be a bug
   * in my implementation, or just a side-effect of the inaccuracy of
   * B due to the discrete nature of the sampling.
   *
   * To overcome this behavior, we'll evaluate which assignment best
   * matches the given eigenvectors by comparing the curvature
   * estimates computed above and the curvatures calculated from the
   * discrete differential operators.  */

  gts_vertex_principal_curvatures (0.5 * normKh, Kg, &K1, &K2);
  
  err_e1 = err_e2 = 0.0;
  /* loop through the values previously saved */
  for (e = 0; e < edge_count; e++) {
    real weight, kappa, d1, d2;
    real temp1, temp2;
    real delta;

    weight = weights[e];
    kappa = kappas[e];
    d1 = d1s[e];
    d2 = d2s[e];

    temp1 = fabs (eig[0] * d1 + eig[1] * d2);
    temp1 = temp1 * temp1;
    temp2 = fabs (eig[1] * d1 - eig[0] * d2);
    temp2 = temp2 * temp2;

    /* err_e1 is for K1 associated with e1 */
    delta = K1 * temp1 + K2 * temp2 - kappa;
    err_e1 += weight * delta * delta;

    /* err_e2 is for K1 associated with e2 */
    delta = K2 * temp1 + K1 * temp2 - kappa;
    err_e2 += weight * delta * delta;
  }
  free (weights);
  free (kappas);
  free (d1s);
  free (d2s);

  /* rotate eig by a right angle if that would decrease the error */
  if (err_e2 < err_e1) {
    real temp = eig[0];

    eig[0] = eig[1];
    eig[1] = -temp;
  }

  e1[0] = eig[0] * basis1[0] + eig[1] * basis2[0];
  e1[1] = eig[0] * basis1[1] + eig[1] * basis2[1];
  e1[2] = eig[0] * basis1[2] + eig[1] * basis2[2];
  e1.normalize();

  /* make N,e1,e2 a right handed coordinate sytem */
  e2 =  N ^ e1;
  e2.normalize();
}

namespace OGF {
    inline static real angle(WOEdge * h) {
        const Vec3r& n1 = h->GetbFace()->GetNormal();
        const Vec3r& n2 = h->GetaFace()->GetNormal();
        const Vec3r v = h->getVec3r();
        real sine = (n1 ^ n2) * v / v.norm() ;
        if(sine >= 1.0) {
            return M_PI / 2.0 ;
        }
        if(sine <= -1.0) {
            return -M_PI / 2.0 ;
        }
        return ::asin(sine) ;
    }

    // precondition1: P is inside the sphere
    // precondition2: P,V points to the outside of 
    //   the sphere (i.e. OP.V > 0)
    static bool sphere_clip_vector(
        const Vec3r& O, real r,
        const Vec3r& P, Vec3r& V
    ) {

        Vec3r W = P - O ;
        real a = V.squareNorm() ;
        real b = 2.0 * V * W ;
        real c = W.squareNorm() - r*r ;
        real delta = b*b - 4*a*c ;
        if(delta < 0) {
            // Should not happen, but happens sometimes (numerical precision)
            return true ;
        }
        real t = - b + ::sqrt(delta) / (2.0 * a) ;
        if(t < 0.0) {
            // Should not happen, but happens sometimes (numerical precision)
            return true ;
        }
        if(t >= 1.0) {
            // Inside the sphere
            return false ;
        }

        V[0] = (t * V.x()) ;
        V[1] = (t * V.y()) ;
        V[2] = (t * V.z()) ;

        return true ;
    }

    // TODO: check optimizations:
    // use marking ? (measure *timings* ...)
    void compute_curvature_tensor(
        WVertex* start, real radius, NormalCycle& nc
    ) {
        // in case we have a non-manifold vertex, skip it...
        if(start->isBoundary())
          return;
      
        std::set<WVertex*> vertices ;
        const Vec3r& O = start->GetVertex() ;
        std::stack<WVertex*> S ;
        S.push(start) ;
        vertices.insert(start) ;
        while(!S.empty()) {
            WVertex* v = S.top() ;
            S.pop() ;
            if(v->isBoundary())
              continue;
            const Vec3r& P = v->GetVertex() ;
            WVertex::incoming_edge_iterator woeit = v->incoming_edges_begin();
            WVertex::incoming_edge_iterator woeitend = v->incoming_edges_end();
            for(;woeit!=woeitend; ++woeit){
              WOEdge *h = *woeit;
              if((v == start) || h->GetVec() * (O - P) > 0.0) {
                    Vec3r V(-1 * h->GetVec());
                    bool isect = sphere_clip_vector(O, radius, P, V) ;
                    assert (h->GetOwner()->GetNumberOfOEdges() == 2); // Because otherwise v->isBoundary() would be true
                    nc.accumulate_dihedral_angle(V, h->GetAngle()) ;

                    if(!isect) {
                        WVertex* w = h->GetaVertex() ;
                        if(vertices.find(w) == vertices.end()) {
                            vertices.insert(w) ;
                            S.push(w) ;
                        }
                    }
                }
            }
        }
    }


    void compute_curvature_tensor_one_ring(
        WVertex* start, NormalCycle& nc
    ) {
        // in case we have a non-manifold vertex, skip it...
        if(start->isBoundary())
          return;
      
        WVertex::incoming_edge_iterator woeit = start->incoming_edges_begin();
        WVertex::incoming_edge_iterator woeitend = start->incoming_edges_end();
        for(;woeit!=woeitend; ++woeit){
          WOEdge *h = (*woeit)->twin();
          nc.accumulate_dihedral_angle(h->GetVec(), h->GetAngle()) ;
          WOEdge *hprev = h->getPrevOnFace();
          nc.accumulate_dihedral_angle(hprev->GetVec(), hprev->GetAngle()) ;
        }
    }

}