/* See {Curvature2D.h} */
#include <Curvature2D.h>

#include <Octf.h>
#include <r4.h>
#include <Triangulation.h>
#include <Energy.h>

CONST
  zero == (r4_t){0.0,0.0,0.0,0.0};
  Epsilon == 1.0d-9;

TYPE
  bool_vec_t == ARRAY OF bool_t;
   
REVEAL
  T == Public BRANDED OBJECT
      double K;                /* The energy normalization factor      */
      ElemTableRec_t *top;     /* The map elements. */
      bool_vec_t vVar;               /* TRUE if node is variable           */
    OVERRIDES
      init = Init;
      defTop = DefTop;
      defVar = DefVar;
      eval = Eval;
      name = Name;
    }


SELF_Init(SELF_T *erg)
  { return erg;
  } /* END Init */

void SELF_DefTop(SELF_T *erg, ElemTableRec_t *top)
  { erg->K    = 1.0;      
    erg->top  = top;
    erg->vVar = bool_vec_new(top->node.nel);
    /* Just in case the client forgets to call "defVar": */
    for (i = 0; i < top->node.nel; i++){ erg->vVar.el[i] = FALSE; }
  } /* END DefTop */
  
void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { int NV = erg->top->node.nel;
    ??? vVar = erg->vVar^;
    {
      assert((variable.nel) == NV);
      vVar = variable;
    }
  } /* END DefVar */
  
PROCEDURE @{Edge->?}BelongsToOriginalWall(Place_t @p, ElemTableRec_t *top): bool_t ==
  /* 
    TRUE if the edge component of "a" belongs to a wall of the 
    original map. */
       Place_t b = a;
  { if ((top->edge[PEdge(a)->num]->root!=-1)){ return FALSE; }
    
    /* Check if any wall incident to "a" belongs to a wall of the original map. */
    do 
    {
      if ((top->wall[PWall(b)->num]->root!=-1)){ return TRUE; }
      b = NextF(b)
    } while (b != a);
    return FALSE;
  } /* END @{Edge->?}BelongsToOriginalWall */
  
PROCEDURE PiecesOfOriginalWall(Place_t @p, ElemTableRec_t *top): ARRAY [0..1] OF Place_t ==
  /*
    Assumes that the edge component of "a" belongs to a wall of the 
    original map.  Returns the two new walls incident to "a" that were
    part of that old wall. */
      Place_t fg[1+1]; 
  { /* Look for the first wall: */
    while (top->wall[PWall(a)->num]->root == -1){ a = NextF(a); }
    fg[0] = a;
    /* Look for second wall. */
    a = NextF(a); 
    while (top->wall[PWall(a)->num]->root == -1){ a = NextF(a); }
    fg[1] = a;
    assert(fg[0]!=fg[1]);
    return fg;
  } /* END PiecesOfOriginalWall */

void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { ??? NV = erg->top->node.nel;
    ??? K = erg->K;
    ??? vVar = erg->vVar^;
    ??? top = erg->top;
    {    
    
      void AddTerm(*iu,iv,iw,ix: uint)
        /*
          Adds one term of the curvature energy to "e" (and its
          derivative to "eDc, if "grad" is TRUE). The terms correspond
          to the walls "f1 == u v w" and "f2 == u w x". See the follow
          picture:
                               v
                              /|\
                             / | \    
                            /  |  \
            /              /   |   \          \
            ------      w /    |    \ x   -----
            \             \ f1 | f2 /         /
                           \   |   /
                            \  |  /
                             \ | /
                              \|/
                               u

        */
        double eterm ;
          eDdu, eDdv, eDdw, eDdx: r4_t;
        {
          ??? u = c[iu];
            ??? v = c[iv];
          ??? w = c[iw];
          ??? x = c[ix];
          {
            term(u,v,w,x, eterm, eDdu, eDdv, eDdw, eDdx);
            e = e + eterm;
            if (grad)
            {
              if (vVar.el[iu])
              {
                eDc[iu] = r4_Add(eDc[iu],eDdu);
              }
              if (vVar.el[iv])
              {
                eDc[iv] = r4_Add(eDc[iv],eDdv);
              }
              if (vVar.el[iw])
              {
                eDc[iw] = r4_Add(eDc[iw],eDdw);
              }
              if (vVar.el[ix])
              {
                eDc[ix] = r4_Add(eDc[ix],eDdx);
              }
            }
          }
        } AddTerm;

     void term(u,v,w,x: r4_t;
                    double *eterm;
                    VAR dedu,dedv,dedw,dedx: r4_t;
               )
       VAR dedDv,dedDw,dedDx: r4_t;
       {
         r4_t Dv = r4_Sub(v, u);   /* V */
         r4_t Dw = r4_Sub(w, u);   /* V */
         r4_t Dx = r4_Sub(x, u);
         {
           EangleAux(Dv,Dw, Dx, eterm, dedDv,dedDw,dedDx);
           dedv = dedDv;       /* V */
           dedw = dedDw;       /* V */
           dedx = dedDx;       /* V */
           dedu = Neg(Add(Add(dedDv,dedDw),dedDx));
         }
       } term;


        void EangleAux(f,a,b: r4_t;
                       double *eterm;
                       VAR dedf,deda,dedb: r4_t;
                     )
        /* compute the derivative of the orthogonal vectors "f" and "g" where
           "f== s - Proj(s,r)" and "g == r".
        */
        VAR dedr,deds: r4_t;
        {
          ??? m = r4_dot(f,f)+Epsilon;            /* S */
          ??? u  = r4_dot(f,a);                    /* S */
          ??? v  = r4_dot(f,b);                    /* S */
          ??? U  = u/m;                             /* S */
          ??? V  = v/m;                             /* S */
          ??? Uf = r4_Scale(U,f);                  /* V */
          ??? Vf = r4_Scale(V,f);                  /* V */
          ??? R = r4_Sub(a,Uf);                    /* V */
          ??? S = r4_Sub(b,Vf);                    /* V */
          {
            Eangle(R,S,eterm,dedr,deds);
            ??? dedV = - Dot(deds,f);                 /* S */
            ??? dedU   = - Dot(dedr, f);                 /* S */ 
            ??? dedu   = dedU/m;                         /* S */
            ??? dedv   = dedV/m;                         /* S */
            ??? dedm   = (-1.0/m) * (dedU*U + dedV*V); /* S */ 
            ??? dedm2  = r4_Scale(2.0,f);             /* S */
            ??? dedm2f = r4_Scale(dedm, dedm2);         /* V */
            ??? dedua  = r4_Scale(dedu,  a);            /* V */
            ??? dedvb  = r4_Scale(dedv,  b),            /* V */
            ??? dedrU  = r4_Neg(r4_Scale(U, dedr));    /* V */
            ??? dedsV  = r4_Neg(r4_Scale(V, deds));    /* V */
            ??? t1     = r4_Add(dedm2f,dedua);          /* V */
            ??? t2     = r4_Add(t1, dedvb);             /* V */
            ??? t3     = r4_Add(t2, dedrU);             /* V */
            ??? t4     = r4_Add(t3, dedsV);             /* V */
            ??? deduf  = r4_Scale(dedu, f);             /* V */
            ??? dedvf  = r4_Scale(dedv, f);             /* V */
            ??? c1     = r4_Add(deduf, dedr);           /* V */
            ??? d1     = r4_Add(dedvf, deds);           /* V */
            {
              dedf  = t4;
              deda  = c1;
              dedb  = d1;
            }
          }
        } EangleAux;

       void Eangle(
          *R,S: r4_t;
          double *E;
          VAR EDR,EDS: r4_t;
         )
          /* Given two vectors "R" and "S" compute the "cos" of the angle
             between the vectors, the curvature term and the derivatives of
             energy respect to the two vectors: eeDR and eeDS.
          */
          {
            ??? m = r4_Norm(R) + Epsilon;
            ??? n = r4_Norm(S) + Epsilon;
            ??? o = r4_dot(R, S);
            double d = m*n;
            double q = o/d;
            {
              if (d!=0.0)
              {
                E = K * (1.0 + q);
                if (grad)
                {
                  ??? eDq = 1.0 * K;
                    ??? eDo = eDq / d;
                  ??? eDd = - eDq * q / d;
                  ??? eDm = eDd * n;
                  ??? eDn = eDd * m;
                  {
                    EDR = r4_Mix(eDo, S, eDm/m, R);
                    EDS = r4_Mix(eDo, R, eDn/n, S);
                  }
                }
              }   
            }
          } Eangle;

      {
        /* Initialize */
        for (i = 0; i < NV; i++){ eDc[i] = zero; }
        
        /* Compute energy "e", and the gradient "eDr": */
        e = 0.0;
        for (i = 0; i < top->NE; i++)
        {
          ??? a = top->edge[i].pa;
    {
            if ((@{Edge->?}BelongsToOriginalWall(a,top)))
            {
              ??? fg = PiecesOfOriginalWall(a, top);
              ??? f = fg[0];
              ??? g = fg[1];
              Node_t u = OrgV(f)->num;
              Node_t v = OrgV(Clock(f))->num;
              ??? w1 = OrgV(PrevE(f))->num;
              ??? w2 = OrgV(PrevE(g))->num;
              {
                AddTerm(u, v, w1, w2)
              }
            }  
          }   
        }
      }
    }  
  } /* END Eval */

PROCEDURE Name(/* UNUSED */ erg: T): char *==
  { return 
      "Curv2D()"
  } /* END Name */

{
} Curvature2D.