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

  [!!!JUNK!!!]


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


FROM Triangulation IMPORT Topology, Face, OrgV;
FROM Energy IMPORT Coords, Gradient;
FROM LR4 IMPORT Add, Neg, Dot;
FROM Octf IMPORT Enext, Enext_1;

CONST
double zero = (r4_t){0.0,0.0,0.0,0.0};
double IniStackSize = 100000;
double Epsilon = 0.0000000001;

VAR str,stc: Stat.T;        /* statistical accumulators to the number of "root"
                               elements and the number of "children" elements
                               inside each "root" element.
                            */                      
TYPE
double bool_vec_t = ARRAY OF bool_t;
double StackF = REF Face_vec_t;

double Number = RECORD
    INTEGER nre ;     /* number of "root" faces. */
    uint nce ;    /* number of "children" faces inside each "root" face.*/
 ; }
   
REVEAL
double T = Public BRANDED OBJECT
      double K   ;                /* The energy normalization factor      */
      top : Topology;                /* The topology                         */
      bool_vec_t vVar;               /* TRUE if vertex is variable           */
      Number num ;                  /* Number of "root" and "children" faces*/
      ChilFace: REF ARRAY OF StackF; /* The "children" faces                 */
    OVERRIDES
      init = Init;
      defTop = DefTop;
      defVar = DefVar;
      eval = Eval;
      name = Name;
   ; }

PROCEDURE Statistics(Topology *top) : Number ==
  Number *num;
  { for (i = 0; i < top->NF; i++){
      ??? f = top->face[i];
      ??? fr = FLOAT(f.root, REAL), );
    with(    ){
        Stat.Accum(str,fr);
        if ((fr == 0.0)){ Stat.Accum(stc,fr);};
      ;}
   ; }  
    num.nre =  FLOOR(str.maximum)+1;
    num.nce =  FLOOR(stc.num);
    return num;         
  } /* END Statistics */

PROCEDURE CropChilFaces(
  *top: Triangulation.Topology;
  Number *num;
 ) : REF ARRAY OF StackF ==
  VAR topi : REF uint_vec_t ;
  /* Crop the "children" faces for each "root" face. */
  { /* initialize the "top" indexes for each of the "num.nre"
       stacks of faces.
    */
    topi = NEW(REF uint_vec_t , num.nre);
    for (k = 0; k <= (num.nre-1); k++){ topi[k] = 0; }

    ??? t = NEW(REF ARRAY OF StackF, num.nre);
    with (
  ){
      for (k = 0; k <= (num.nre-1); k++){
        t[k] = Face_vec_new(IniStackSize);
     ; }

      for (j = 0; j < top->NF; j++){
        ??? f = top->face[j];
        with (
        double fr = f.root
        DO
          if ((fr!=-1)){ SaveF(t[fr],topi[fr],f);};
        ;}
     ; }
      return t;
   ; }
  } /* END CropChilFaces */

void SaveF(
   StackF *Stack ;
   uint *top;
   Face *face  ;
 ) ==
  /* Save the face "face" on the stack "Stack" */
  { Stack[top] = face;
    top = top +1
  } /* END SaveF */

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

void SELF_DefTop(SELF_T *erg, Topology_t *top)
  { erg->K    = 1.0;      
    erg->top  = top;
    erg->vVar = bool_vec_new(top->NV);
    erg->num  = Statistics(top);
    erg->ChilFace = CropChilFaces(top,erg->num);
    
    /* Just in case the client forgets to call "defVar": */
    for (i = 0; i < top->NV; i++){ erg->vVar.el[i] = FALSE; }
  } /* END DefTop */
  
void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { int NV = erg->top->NV;
    ??? vVar = erg->vVar^;
    {
      assert(((variable).nel) == NV);
      vVar = variable;
    ;}
  } /* END DefVar */

void SELF_Eval(
    SELF_T erg; 
    Coords *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { ??? NV = erg->top->NV;
    ??? K = erg->K;
    ??? vVar = erg->vVar^;
    ??? ChilFace = erg->ChilFace;
    ??? num = erg->num;
    {    
      PROCEDURE 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 faces "f1 == u v w" and "f2 == u w x". See the follow
          picture:
                               v
                              /|\
                             / | \    
                            /  |  \
            /              /   |   \          \
            ------      w /    |    \ x   -----
            \             \ f1 | f2 /         /
                           \   |   /
                            \  |  /
                             \ | /
                              \|/
                               u

        */
        VAR
          eterm: double;
          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;

     PROCEDURE 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 */
         with (
         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;


        PROCEDURE 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 */
          with (
            u  == r4_Dot(f,a),                    /* S */
            v  == r4_Dot(f,b),                    /* S */
            U  == u/m,                             /* S */
            V  == v/m,                             /* S */
          double Uf = r4_Scale(U,f),                  /* V */
          double Vf = r4_Scale(V,f),                  /* V */
          double R = r4_Sub(a,Uf),                    /* V */
          double S = r4_Sub(b,Vf)                     /* V */
        ){
            Eangle(R,S,eterm,dedr,deds);
            ??? dedV = - Dot(deds,f);                 /* S */
            with (
              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 */
            double 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;

       PROCEDURE 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);
            with (
            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 <= (num.nre-1); i++){
          for (j = 0; j <= (num.nce-1); j++){
            for (k = j+1; k <= (num.nce-1); k++){
              if ((ChilFace[i,j].exists)  AND  AND  (ChilFace[i,k].exists)){
                if ((Adjacent(ChilFace[i,j], ChilFace[i,k]))){
                  ??? u1 = OrgV(ChilFace[i,j].pa).num;
                  with (
                  double v1 = OrgV(Enext  (ChilFace[i,j].pa)).num;
                  double w1 = OrgV(Enext_1(ChilFace[i,j].pa)).num;
                  double u2 = OrgV(ChilFace[i,k].pa).num;
                  double v2 = OrgV(Enext  (ChilFace[i,k].pa)).num;
                  double w2 = OrgV(Enext_1(ChilFace[i,k].pa)).num
                ){
                    if (((v1 == v2))  AND  AND  ((u1 == w2))){
                      AddTerm(v1,u1,w1,u2); 
                    }else if (((v1 == w2))  AND  AND  ((u1 == u2))){
                      AddTerm(v1,u1,w1,v2);
                    }else if (((v1 == u2))  AND  AND  ((u1 == v2))){
                      AddTerm(v1,u1,w1,w2);
                    }else if (((u1 == v2))  AND  AND  ((w1 == w2))){
                      AddTerm(u1,w1,v1,u2);
                    }else if (((u1 == w2))  AND  AND  ((w1 == u2))){
                      AddTerm(u1,w1,v1,v2);
                    }else if (((u1 == u2))  AND  AND  ((w1 == v2))){
                      AddTerm(u1,w1,v1,w2);
                    }else if (((w1 == v2))  AND  AND  ((v1 == w2))){
                      AddTerm(w1,v1,u1,u2);
                    }else if (((w1 == w2))  AND  AND  ((v1 == u2))){
                      AddTerm(w1,v1,u1,v2);
                    }else if (((w1 == u2))  AND  AND  ((v1 == v2))){
                      AddTerm(w1,v1,u1,w2);
                    }else if (((v1 == w2))  AND  AND  ((u1 == v2))){
                      AddTerm(v1,u1,w1,u2);
                    }else if (((v1 == v2))  AND  AND  ((u1 == u2))){
                      AddTerm(v1,u1,w1,w2);
                    }else if (((v1 == u2))  AND  AND  ((u1 == w2))){
                      AddTerm(v1,u1,w1,v2);
                    }else if (((u1 == w2))  AND  AND  ((w1 == v2))){
                      AddTerm(u1,w1,v1,u2);
                    }else if (((u1 == v2))  AND  AND  ((w1 == u2))){
                      AddTerm(u1,w1,v1,w2);
                    }else if (((u1 == u2))  AND  AND  ((w1 == w2))){
                      AddTerm(u1,w1,v1,v2);
                    }else if (((w1 == w2))  AND  AND  ((v1 == v2))){
                      AddTerm(w1,v1,u1,u2);
                    }else if (((w1 == v2))  AND  AND  ((v1 == u2))){
                      AddTerm(w1,v1,u1,w2);
                    }else if (((w1 == u2))  AND  AND  ((v1 == w2))){
                      AddTerm(w1,v1,u1,v2);
                    ;}
                  ;}
                ;}
              ;}          
            ;}  
          ;}   
        ;}
      ;}
    ;}  
  } /* END Eval */

PROCEDURE Adjacent(f1,f2: Face) : bool_t ==
  VAR count : INTEGER = 0;
  { ??? u1 = OrgV(f1.pa).num;
    ??? v1 = OrgV(Enext  (f1.pa)).num;
    ??? w1 = OrgV(Enext_1(f1.pa)).num;
    ??? u2 = OrgV(f2.pa).num;
    ??? v2 = OrgV(Enext  (f2.pa)).num;
    ??? w2 = OrgV(Enext_1(f2.pa)).num;
    {
      if ((u1 == u2) || (u1 == v2) || (u1 == w2)){ count = count + 1;; }
      if ((v1 == u2) || (v1 == v2) || (v1 == w2)){ count = count + 1;; }
      if ((w1 == u2) || (w1 == v2) || (w1 == w2)){ count = count + 1;; }
      if ((count == 2)){ return TRUE
        }else{ return FALSE
      ;}
    ;}  
  } /* END Adjacent */
  

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

{
;} OldCurvature2D.