/* See {Curvature1D.h} */
#include <Curvature1D.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;
  StackE == REF Edge_vec_t;
    
  Number == RECORD
    INTEGER nre;     /* number of "root" @{edge->?}s. */
    uint nce;    /* number of "children" @{edge->?}s inside each "root" @{edge->?}.*/
  }
   
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" @{edge->?}s*/
      Chil@{Edge->?}: REF ARRAY OF StackE; /* The "children" @{edge->?}s                 */
    OVERRIDES
      init = Init;
      defTop = DefTop;
      defVar = DefVar;
      eval = Eval;
      name = Name;
    }

void SaveE(
   StackE *Stack;
   uint *top;
   @{Edge->?} *@{edge->?};
 )
  /* Save the edge "@{edge->?}" on the stack "Stack" */
  { Stack.El[top] = @{edge->?};
    top = top +1
  } /* END SaveE */

Number Statistics(ElemTableRec_t *top)
  Number *num;
  { for (i = 0; i < top->NE; i++){
      ??? e = top->edge[i];
      double = e->root;
      {
        Stat.Accum(str,er);
        if (er == 20.0){ Stat.Accum(stc,er); } /* 0.0 */
      }
    }  
    num.nre =  FLOOR(str.maximum)+1;
    num.nce =  FLOOR(stc->num);
    return num;         
  } /* END Statistics */

PROCEDURE CropChil@{Edge->?}s(
 ElemTableRec_t *top;
  Number *num;
 ): REF ARRAY OF StackE ==
  /* Crop the "children" @{edge->?}s for each "root" @{edge->?}. */
      topj : REF uint_vec_t;
  { /* initialize the "top" indexes for each of the "num.nre"
       stacks of @{edge->?}s.
    */
    topj = NEW(REF uint_vec_t , num.nre);
    for (k = 0; k < num.nre; k++){ topj[k] = 0; }

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

      for (j = 0; j < top->NE; j++)
      {
        ??? e = top->edge[j];
        int er = e->root;
        DO
          if (er!=-1){ SaveE(t[er],topj[er],e); }  
        }
      }
      return t;
    }
  } /* END CropChil@{Edge->?}s */

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->Chil@{Edge->?} = CropChil@{Edge->?}s(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^;
    ??? Chil@{Edge->?} = erg->Chil@{Edge->?};
    ??? num = erg->num;
    {    
      void AddTerm(*iu,iv,iw: uint)
        /*
          Adds one term of the curvature energy to "e" (and its
          derivative to "eDc, if "grad" is TRUE). The terms correspond
          to the edge "e1 == u v" and the edge "e2 == u w".

                     v        w
                      \      /
                       \    /
                    e1  \  / e2
                         \/
                          u
        */
        double eterm ;
          eDdu, eDdv, eDdw: r4_t;
        {
          ??? u = c[iu];
            ??? v = c[iv];
          ??? w = c[iw];
          {
            term(u,v,w, eterm, eDdu, eDdv, eDdw);
            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);
              }
            }
          }
        } AddTerm;

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

       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.
          */
          {
            double m = r4_Norm(R) + Epsilon;
            double n = r4_Norm(S) + Epsilon;
            double 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 "eDc": */
        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 ((Chil@{Edge->?}[i,j]->exists) && (Chil@{Edge->?}[i,k]->exists))
              {
               if ((Adjacent(Chil@{Edge->?}[i,j], Chil@{Edge->?}[i,k])))
               {
                  uint u1 = OrgV(Chil@{Edge->?}[i,j].pa)->num;
                  uint v1 = OrgV(Clock(Chil@{Edge->?}[i,j].pa))->num;
                  uint u2 = OrgV(Chil@{Edge->?}[i,k].pa)->num;
                  uint v2 = OrgV(Clock(Chil@{Edge->?}[i,k].pa))->num;
                  {
                    if (u1 == u2)
                    {
                      AddTerm(u1,v1,v2);
                    }
                    else
                     if (v1 == u2){
                      AddTerm(v1,u1,v2);
                    }
                    else
                     if (u1 == v2){
                      AddTerm(u1,v1,u2);
                    }
                    else
                     if (v1 == v2){
                      AddTerm(v1,u1,u2);
                    }
                  }
                }
              }          
            }  
          }   
        }
      }
    }  
  } /* END Eval */

bool_t Adjacent(e1,e2: @{Edge->?})
  { ??? u1 = OrgV(e1.pa)->num;
    ??? v1 = OrgV(Clock(e1.pa))->num;
    ??? u2 = OrgV(e2.pa)->num;
    ??? v2 = OrgV(Clock(e2.pa))->num;
    {
      if ((u1 == u2) || (u1 == v2) || (v1 == v2) || ((v1 == u2)
      )){ return TRUE;
        }
        else
        { return FALSE;
      }
    }  
  } /* END Adjacent */

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

{
} Curvature1D.