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

#define Curvature3D_C_author \
  "Modified by L.A.P.Lozada on 2000-11-18."

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

CONST
  zero    == (r4_t){0.0,0.0,0.0,0.0};
  Epsilon == 0.0000000001;
  
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      */
      bool_vec_t wallRelevant;           /* TRUE if wall is relevant        */ 

    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);
    erg->wallRelevant = bool_vec_new(top->wall.nel);
    /* Just in case the client forgets to call "defVar": */
    for (i = 0; i < top->node.nel; i++){ erg->vVar.el[i] = FALSE; }
    for (i = 0; i < top->wall.nel; i++){ erg->wallRelevant.el[i] = FALSE; }    
  } /* END DefTop */
  
void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { /*
      Decide which wall are relevant to the curvature  energy. A wall is
      relevant iff has exactly two cell incident to it.
    */
    int NV = erg->top->node.nel;
    int NF = erg->top->wall.nel;
    bool_vec_t vVar = erg->vVar^;
    ??? wall = erg->top->wall^;
    ??? wallRelevant = erg->wallRelevant^;
    {
      assert((variable.nel) == NV);
      vVar = variable;
      
      /* Find the relevant walls: */
      for (i = 0; i < NF; i++){ wallRelevant.el[i] = FALSE; }
      for (i = 0; i < NF; i++)
      {
        ??? f = wall[i];
          ??? a = f.pa;
        ??? p1 = PnegP(a);
        ??? p2 = PposP(a);
        {
          if (((p1!=NULL) && (p2!=NULL)))
          {
            wallRelevant.el[i] = TRUE; 
          }
        }
      }
    }
  } /* END DefVar */

void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { int NV = erg->top->node.nel;
    int NF = erg->top->wall.nel;
    ??? wall = erg->top->wall^;
    ??? wallRelevant = erg->wallRelevant^;
    ??? K = erg->K;
    ??? vVar = erg->vVar^;
    {    
      void AddTerm(*iu,iv,iw,ix,iy: uint)
        /*
          Adds one term of the curvature energy to "e" (and its
          derivative to "eDc, if "grad" is TRUE). The terms correspond
          to the wall "f == u v w" shared by the tetrahedra "u v w x" (PnegP)
          and "u v w y" (PposP).
                     _                                 _
                    |\         w               w       /|              
                      \       /|\             /|\     /
                       \     / | \           / | \   /
                            /  |  \         /  |  \
                           /   |   \       /   |   \
                        x /____|____\v   v/____|____\ y
                          \    | f  /     \  f |    /
                           \   |   /       \   |   /
                            \  |  /         \  |  /
                             \ | /           \ | /
                              \|/             \|/
                               u               u

                              PnegP           PposP

        */
        double eterm ;
          eDdu, eDdv, eDdw, eDdx, eDdy: r4_t;
        {
          ??? u = c[iu];
            ??? v = c[iv];
          ??? w = c[iw];
          ??? x = c[ix];
          ??? y = c[iy];
          {
            term(u,v,w,x,y, eterm, eDdu, eDdv, eDdw, eDdx, eDdy);
            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);
              }
              if (vVar.el[iy])
              {
                eDc[iy] = r4_Add(eDc[iy],eDdy);
              }
            }
          }
        } AddTerm;

     void term(u,v,w,x,y: r4_t;
                    double *eterm;
                    VAR dedu,dedv,dedw,dedx,dedy: r4_t;
               )
       VAR dedDv,dedDw,dedDx,dedDy: 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);   /* V */
         r4_t Dy == r4_Sub(y,u);   /* V */
         {
           Eangle(Dv,Dw,Dx,Dy, eterm, dedDv,dedDw,dedDx,dedDy);
           dedv = dedDv;       /* V */
           dedw = dedDw;       /* V */
           dedx = dedDx;       /* V */
           dedy = dedDy;       /* V */
           dedu = Neg(Add(Add(Add(dedDv,dedDw),dedDx),dedDy));
         }
       } term;

      void Eangle(r,s,a,b: r4_t;
                       double *eterm;
                       VAR dedr,deds,deda,dedb: r4_t;
                     )
        /* compute the derivative of the orthogonal vectors "f" and "g" where
           "f== s - Proj(s,r)" and "g == r".
        */
        VAR dedf,dedg: r4_t;
        {
          ??? m = Dot(r,r)+Epsilon;    /* S */
          ??? n  = Dot(r,s);            /* S */
          ??? m2 = m * m;               /* S */
          ??? q  = n/m;                 /* S */
          ??? qr = Scale(q,r);          /* V */
          ??? f  = Sub(s,qr);           /* V */
          ??? g  = r;                   /* V */
          /* Now, "f" and "g" are orthogonal */
          {
            EangleAux(f,g,a,b,eterm,dedf,dedg,deda,dedb);
            ??? dedq = -r4_dot(dedf,r);          /* S */
            ??? dedn = dedq/m;                   /* S */
            ??? dedm = -dedq * (n/m2);           /* S */
            ??? r2   = r4_Scale(2.0,r);        /* V */ 
            ??? v1   = r4_Scale(dedm, r2);       /* V */
            ??? v2   = r4_Scale(dedn,  s);       /* V */
            ??? v3   = r4_Scale(-q , dedf);      /* V */
            ??? v4   = dedg;                     /* V */
            ??? v12  = r4_Add(v1,v2);            /* V */
            ??? v123 = r4_Add(v12,v3);           /* V */
            ??? v1234= r4_Add(v123,v4);          /* V */
            ??? dednr == r4_Scale(dedn,  r);     /* V */
            {
              dedr = v1234;
              deds = Add(dednr, dedf);
            }
          }
        }  Eangle;

        void EangleAux(
          f,g,a,b: r4_t;
          double *eterm;
          VAR dedf,dedg,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 */
          double n  = r4_dot(g,g) + Epsilon,          /* S */
          double u  = r4_dot(f,a);                    /* S */
          double v  = r4_dot(f,b);                    /* S */
          double x  = r4_dot(g,a);                    /* S */
          double y  = r4_dot(g,b);                    /* S */
          double U  = u/m;                             /* S */
          double V  = v/m;                             /* S */
          double X  = x/n;                             /* S */
          double Y  = y/n;                             /* S */
          double Uf = r4_Scale(U,f);                  /* V */
          double Xg = r4_Scale(X,g);                  /* V */
          double Vf = r4_Scale(V,f);                  /* V */
          double Yg = r4_Scale(Y,g);                  /* V */
          double UfXg = r4_Neg(r4_Add(Uf,Xg));       /* V */
          double VfYg = r4_Neg(r4_Add(Vf,Yg));       /* V */
          double R = r4_Add(a,UfXg);                  /* V */
          double S = r4_Add(b,VfYg);                  /* V */
          {
            EangleVec(R,S,eterm,dedr,deds);
            ??? dedY = - Dot(deds,g);                 /* S */
            ??? dedX   = - Dot(dedr, g);                 /* S */
            ??? dedV   = - Dot(deds, f);                 /* S */
            ??? dedU   = - Dot(dedr, f);                 /* S */ 
            ??? dedx   = dedX/n;                         /* S */
            ??? dedy   = dedY/n;                         /* S */
            ??? dedu   = dedU/m;                         /* S */
            ??? dedv   = dedV/m;                         /* S */
            ??? dedm   = (-1.0/m) * (dedU*U + dedV*V); /* S */ 
            ??? dedn   = (-1.0/n) * (dedX*X + dedY*Y); /* 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 */
            ??? g2     = r4_Scale(2.0,g);          /* S */
            ??? dedn2g = r4_Scale(dedn, g2);         /* V */
            ??? dedxa  = r4_Scale(dedx,  a);         /* V */
            ??? dedyb  = r4_Scale(dedy,  b);         /* V */
            ??? dedrX  = r4_Neg(r4_Scale(X, dedr)); /* V */
            ??? dedsY  = r4_Neg(r4_Scale(Y, deds)); /* V */
            ??? f1     = r4_Add(dedn2g,dedxa),       /* V */
            ??? f2     = r4_Add(f1, dedyb);          /* V */
            ??? f3     = r4_Add(f2, dedrX);          /* V */
            ??? f4     = r4_Add(f3, dedsY);          /* V */
            ??? deduf  = r4_Scale(dedu, f);          /* V */
            ??? dedxg  = r4_Scale(dedx, g);          /* V */
            ??? dedvf  = r4_Scale(dedv, f);          /* V */
            ??? dedyg  = r4_Scale(dedy, g);          /* V */
            ??? c1     = r4_Add(deduf, dedxg);       /* V */
            ??? c2     = r4_Add(c1, dedr);           /* V */
            ??? d1     = r4_Add(dedvf, dedyg);       /* V */
            ??? d2     = r4_Add(d1, deds);           /* V */
            {
              dedf  = t4;
              dedg  = f4;  
              deda  = c2;
              dedb  = d2;
            }
          }
        } EangleAux;

        void EangleVec(
           *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;
            double c  = (1.0 + Epsilon) - q;
            double c2 = c * c;
            {
              if (d!=0.0)
              {
                E = K * (1.0/c - 0.5);
                if (grad)
                {
                  ??? eDq = 1.0/c2 * 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);
                  }
                }
              }   
            }
          } EangleVec;

      {
        /* Initialize */
        for (i = 0; i < NV; i++){ eDc[i] = zero; }
        
        /* Compute energy "e", and the gradient "eDr": */
        e = 0.0;
        for (i = 0; i < NF; i++)
        {
          if (wallRelevant.el[i])
          {
            ??? f = wall[i];
            ??? a = f.pa;
            ??? t = TetraNegPosNodes(a);
            ??? un = t[0]->num;
            ??? vn = t[1]->num;
            ??? wn = t[2]->num;
            ??? xn = t[3]->num;
            ??? yn = t[4]->num;
            {
              assert(xn!=yn);
              AddTerm(un,vn,wn,xn,yn)
            }  
          }   
        }
      }

    }
  } /* END Eval */

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

{
} Curvature3D.