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


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

CONST
  zero == (r4_t){0.0,0.0,0.0,0.0};
  INIT_STACK_SIZE == 100000;
  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
  bool_vec_t == ARRAY OF bool_t;
  StackF == REF Wall_vec_t;

  Number == RECORD
    INTEGER nre;     /* number of "root" walls. */
    uint nce;    /* number of "children" walls inside each "root" wall.*/
  }
   
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           */
      Number num;                  /* Number of "root" and "children" walls*/
      ChilWall: REF ARRAY OF StackF; /* The "children" walls                 */
    OVERRIDES
      init = Init;
      defTop = DefTop;
      defVar = DefVar;
      eval = Eval;
      name = Name;
    }

Number Statistics(ElemTableRec_t *top)
  Number *num;
  { for (i = 0; i < top->wall.nel; i++){
      ??? f = top->wall[i];
      ??? fr = FLOAT(f->root, REAL);
      {
        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 CropChilWalls(
 ElemTableRec_t *top;
  Number *num;
 ) : REF ARRAY OF StackF ==
      topi : REF uint_vec_t;
  /* Crop the "children" walls for each "root" wall. */
  { /* initialize the "top" indexes for each of the "num.nre"
       stacks of walls.
    */
    topi = NEW(REF uint_vec_t , num.nre);
    for (k = 0; k < num.nre; k++){ topi[k] = 0; }

    ??? t = NEW(REF ARRAY OF StackF, num.nre);
    {
      for (k = 0; k < num.nre; k++)
      {
        t[k] = Wall_vec_new(INIT_STACK_SIZE);
      }

      for (j = 0; j < top->wall.nel; j++)
      {
        ??? f = top->wall[j];
        ??? fr = f->root;
        {
          if (fr!=-1){ SaveF(t[fr],topi[fr],f); }
        }
      }
      return t;
    }
  } /* END CropChilWalls */

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

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->num  = Statistics(top);
    erg->ChilWall = CropChilWalls(top,erg->num);
    
    /* 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 */

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^;
    ??? ChilWall = erg->ChilWall;
    ??? num = erg->num;
    {    
      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);
            ??? d = m*n;
            ??? 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; i++)
        {
          for (j = 0; j < num.nce; j++)
          {
            for (k = j+1; k < num.nce; k++)
            {
              if ((ChilWall[i,j]->exists) && (ChilWall[i,k]->exists))
              {
                if ((Adjacent(ChilWall[i,j], ChilWall[i,k])))
                {
                  ??? u1 = OrgV(ChilWall[i,j].pa)->num;
                  ??? v1 = OrgV(NextE  (ChilWall[i,j].pa))->num;
                  ??? w1 = OrgV(PrevE(ChilWall[i,j].pa))->num;
                  ??? u2 = OrgV(ChilWall[i,k].pa)->num;
                  ??? v2 = OrgV(NextE  (ChilWall[i,k].pa))->num;
                  ??? w2 = OrgV(PrevE(ChilWall[i,k].pa))->num;
                  {
                    if ((v1 == v2) && (u1 == w2))
                    {
                      AddTerm(v1,u1,w1,u2); 
                    }
                    else
                     if ((v1 == w2) && (u1 == u2)){
                      AddTerm(v1,u1,w1,v2);
                    }
                    else
                     if ((v1 == u2) && (u1 == v2)){
                      AddTerm(v1,u1,w1,w2);
                    }
                    else
                     if ((u1 == v2) && (w1 == w2)){
                      AddTerm(u1,w1,v1,u2);
                    }
                    else
                     if ((u1 == w2) && (w1 == u2)){
                      AddTerm(u1,w1,v1,v2);
                    }
                    else
                     if ((u1 == u2) && (w1 == v2)){
                      AddTerm(u1,w1,v1,w2);
                    }
                    else
                     if ((w1 == v2) && (v1 == w2)){
                      AddTerm(w1,v1,u1,u2);
                    }
                    else
                     if ((w1 == w2) && (v1 == u2)){
                      AddTerm(w1,v1,u1,v2);
                    }
                    else
                     if ((w1 == u2) && (v1 == v2)){
                      AddTerm(w1,v1,u1,w2);
                    }
                    else
                     if ((v1 == w2) && (u1 == v2)){
                      AddTerm(v1,u1,w1,u2);
                    }
                    else
                     if ((v1 == v2) && (u1 == u2)){
                      AddTerm(v1,u1,w1,w2);
                    }
                    else
                     if ((v1 == u2) && (u1 == w2)){
                      AddTerm(v1,u1,w1,v2);
                    }
                    else
                     if ((u1 == w2) && (w1 == v2)){
                      AddTerm(u1,w1,v1,u2);
                    }
                    else
                     if ((u1 == v2) && (w1 == u2)){
                      AddTerm(u1,w1,v1,w2);
                    }
                    else
                     if ((u1 == u2) && (w1 == w2)){
                      AddTerm(u1,w1,v1,v2);
                    }
                    else
                     if ((w1 == w2) && (v1 == v2)){
                      AddTerm(w1,v1,u1,u2);
                    }
                    else
                     if ((w1 == v2) && (v1 == u2)){
                      AddTerm(w1,v1,u1,w2);
                    }
                    else
                     if ((w1 == u2) && (v1 == w2)){
                      AddTerm(w1,v1,u1,v2);
                    }
                  }
                }
              }          
            }  
          }   
        }
      }
    }  
  } /* END Eval */

bool_t Adjacent(f1,f2: Wall)
      count : INTEGER = 0;
  { ??? u1 = OrgV(f1.pa)->num;
    ??? v1 = OrgV(NextE  (f1.pa))->num;
    ??? w1 = OrgV(PrevE(f1.pa))->num;
    ??? u2 = OrgV(f2.pa)->num;
    ??? v2 = OrgV(NextE  (f2.pa))->num;
    ??? w2 = OrgV(PrevE(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 */

{
} Curvature2D.