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


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

TYPE
  bool_vec_t == ARRAY OF bool_t;
  double_vec_t == double_vec_t;
  DRF == REF uint_vec_t;
  EndPoints == ARRAY [0..1] OF uint;
  Wall == Triangulation.Wall;
  @{Edge->?} == Triangulation.edge;
  WALLS == Wall_vec_t;
  Walls == REF WALLS;

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 */
      @{edge->?}Relevant: REF bool_vec_t;             /* TRUE if @{edge->?} is relevant */
      da: REF double_vec_t;                       /* The average diedral angles */
      DRF drf;                            /* The Walls Ring Degreee for @{edge->?}*/
      walls: REF Wall_vec_ts;           /* The walls-ring for @{edge->?} */
      zmDbe : REF r4_vec_t;
    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)
  { ??? NP = FLOAT(top->cell.nel, double);
    {
      erg->top = top;
      erg->vVar = bool_vec_new(top->node.nel);
      erg->edgeRelevant = bool_vec_new(top->NE);
      erg->walls = CollectWallsRing(top); 
      erg->drf = ComputeWallRingDegree(erg->walls,top);
      erg->K = 1.0/(2.0*Math.pow(NP,5.0/3.0));
    
      /* Allocate average dihedral angle tables: */
      erg->da = double_vec_new(top->NE);
      erg->zmDbe = r4_vec_new(top->NE);

      /* 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->NE; i++){ erg->edgeRelevant[i] = FALSE; }
    }
  } /* END DefTop */

  Wall_vec_t *CollectWalls(*e: @{Edge->?}; ElemTableRec_t *top)==
     /* Return the set of walls incidents to @{edge->?} "e" */
     uint NT = 0;
       uint ct;
     {
       int NF = top->wall.nel;
         ??? t = Wall_vec_new(NF)^;
       {
         for (i = 0; i < NF; i++)
         {
           ct = 0;
           ??? f = top->wall[i];
             ??? fn = f.node;
           ??? eun = e.node[0]->num;
           ??? evn = e.node[1]->num;
           {
             for (j = 0; j < 3; j++)
             {
               if (((fn[j]->num == eun) || (fn[j]->num == evn))){ ct++; }
             }
             if (ct == 2)
             {
               t[NT] = f;
               NT++;
             }
           } 
         }
         ??? r = Wall_vec_new(NT);
    {
           r^ = SUBARRAY(t,0,NT);
           return r;
         }
       }
     } CollectWalls;

  DRF ComputeWallRingDegree(*walls: REF Wall_vec_ts, ElemTableRec_t *top)
    {
      ??? drf = NEW(DRF, top->NE);
      {
        for (l = 0; l < top->NE; l++)
        {
          ??? fie = walls[l];
          {
            drf^[l] =  ((fie^).nel);
          }
        }
        return drf;
      }
    } ComputeWallRingDegree;

  Wall_vec_ts *CollectWallsRing(ElemTableRec_t *top)==
    {
      ??? walls = NEW(REF Wall_vec_ts, top->NE);
      {
        for (l = 0; l < top->NE; l++)
        {
          ??? e = top->edge[l];
          ??? fie = CollectWalls(e, top);
          {
            walls[l] =  fie;
          }
        }
        return walls;
      }
    } CollectWallsRing;

  /* UNUSED */
  double ComputeWallRingDegreeTotal(*drf: DRF)
    uint n = 0;
    VAR {
      for(l = 0; l < drf.nel; l++)
      {
        ??? d = drf^[l];
        {
          n = n + d;
        }
      }
      return ((double)n);
    } ComputeWallRingDegreeTotal;

void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { /*
      This energy is computed only for edges that have the "hinge" bit set,
      have DegreeRingWalls three or more, and have only existing walls and
      variable nodes incident to them. 
    */
    int NV = erg->top->node.nel;
    ??? NE = erg->top->NE;
    bool_vec_t vVar = erg->vVar^;
    ??? @{edge->?} = erg->top->edge^;
    ??? @{edge->?}Relevant = erg->edgeRelevant^;
    ??? drf = erg->drf^;
    ??? walls = erg->walls^;
    {
      assert((variable.nel) == NV);
      vVar = variable;
      
      /* Find the relevant @{edge->?}s: */
      for (i = 0; i < NE; i++){ @{edge->?}Relevant[i] = FALSE; }

      VAR bool_t fexists = TRUE;
      {
        for (i = 0; i < NE; i++)
        {
          ??? e = @{edge->?}[i];
            ??? u = e.node[0];
          ??? v = e.node[1];
          ??? vvar = vVar.el[u->num]) || (vVar.el[v->num];
          ??? de = drf[e->num];
          ??? cf = walls[e->num];
          {
            for (j = 0; j < de; j++)
            {
              ??? f1 = cf^[j];
                ??? f2 = cf^[(j+1) % de];
              {
                fexists = (f1->exists) && (f2->exists)) && (fexists;
              }
            }
            if ((e.hinge) && (fexists) && (vvar) && ( de >= 3))
            {
              ??? u = NARROW(u, Triangulation.Node);
              ??? v = NARROW(v, Triangulation.Node);
              {
                assert(e->exists);
                assert(u->exists) && (v->exists);
              }
              @{edge->?}Relevant[i] = TRUE;
            }
            else
            {
              @{edge->?}Relevant[i] = FALSE;
            }
          }
        }
      } 
    }
  } /* END DefVar */

void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  { int NV = erg->top->node.nel;
    ??? NE = erg->top->NE;
    ??? @{edge->?} = erg->top->edge^;
    ??? @{edge->?}Relevant = erg->edgeRelevant^;
    ??? da = erg->da^;
    ??? K = erg->K;
    ??? drf = erg->drf^;
    ??? walls = erg->walls^;
    bool_vec_t vVar = erg->vVar^;
    ??? zmDbe = erg->zmDbe^;
    {

      PROCEDURE Extremus@{Edge->?}(*e: @{Edge->?}) : EndPoints ==
        EndPoints *stack;
        {
          ??? eun = e.node[0]->num;
            ??? evn = e.node[1]->num;
          {
            stack.el[0] = eun;
            stack.el[1] = evn;
          return stack;
        }
      } Extremus@{Edge->?};

      void ComputeAverageDiedralAngle(*e: @{Edge->?}; double zm;
                                           VAR VAR zmDbe: r4_t)
        double *z;
            zDbe : r4_t; 
        { 
          ??? fie = walls[e->num];
            ??? de = drf[e->num];
          ??? del = ((double)de);
          ??? bi = Extremus@{Edge->?}(e);
          {
            zm = 0.0;
            zmDbe = (r4_t){0.0, ..};
            for (i = 0; i < de; i++)
            {
              ??? f1 = fie[i];
                ??? af1 = f1.pa^;
              ??? f2 = fie[(i+1) % de];
              ??? af2 = f2.pa^;
              ??? bf1 = WallBarycenter(af1,c);
              ??? bf2 = WallBarycenter(af2,c);
              {
                Compute_z(bf1,bf2,bi,z, zDbe);
                zm = zm + z;
                zmDbe = r4_Add(zmDbe, zDbe);
              }
            }
            zm = zm/del;
            zmDbe = r4_Scale(1.0/del, zmDbe);
          }            
        } ComputeAverageDiedralAngle;


      void Compute_z(*bf1,bf2: r4_t; *e: EndPoints;
                          double *z; VAR zDbe: r4_t)
        /*
          Compute the diedral angle between the walls planes f1 and f2.
        */
        { 
          ??? u = e[0];
          ??? v = e[1];
          ??? be = r4_Scale(1.0/2.0, r4_Add(c[u],c[v]));
          ??? a = r4_Sub(be,bf1);
          ??? b = r4_Sub(be,bf2);
          ??? m = r4_Norm(a);
          ??? n = r4_Norm(b);
          ??? d = m * n;
          ??? o = r4_dot(a,b);
          ??? q = o/d;
          {
            z = sqrt( 2.0 * (1.0-q));     /* Aproximation */
            if (grad)
            {
              ??? zDq = - 2.0;
                ??? zDo = zDq /d;
              ??? zDd = - zDq * q / d;
              ??? zDm = zDd * n;
              ??? zDn = zDd * m;
              ??? zDa = r4_Mix(zDo, b, zDm/m, a);
              ??? zDb = r4_Mix(zDo, a, zDn/n, b);
              {
                zDbe = r4_Neg(r4_Add(zDa, zDb));
              }
            }
          }
        } Compute_z;
        
      void Accum_e_from_ee(zm: double; d: uint;
                                double *e; VAR eDzm: double)
        /*
          Adds to "e" the energy term corresponding to hinge @{edge->?} with
          endpoints "e" and average dihedral angle "zm".
        */
        {
          ??? idealtetha = 360.0/((double)d);
            ??? tetha = fabs(zm-idealtetha);
          {
            e = e + K * tetha;
            if (grad)
            {
              eDzm = K;
            }
            else
            {
              eDzm = 0.0;
            }
          }
        } Accum_e_from_ee;

      void Distribute_eDzm(u,v: uint;
                                double *eDzm;
                                *zmDbe: r4_t)
        /* Distribute eDzm on endpoints "c[u]" and "c[v]" */
        {
          ??? eDv = eDc[v];
          ??? eDu = eDc[u];
          ??? eDbe = r4_Scale(eDzm, zmDbe);
          {
            ??? eDcu = r4_Scale(1.0/2.0, eDbe);
            ??? eDcv = r4_Scale(1.0/2.0, eDbe);
            {
              if (vVar.el[v])
              {
                eDv = r4_Add(eDv, eDcv);
              }

              if (vVar.el[u])
              {
                eDu = r4_Add(eDu, eDcu);
              }
            }
          }
        } Distribute_eDzm;

     VAR
       double eDzm;
     {
       /* Clear average dihedral angle accumulators: */
       for (j = 0; j < NE; j++){ da[j] = 0.0;  zmDbe[j] = (r4_t){0.0,.. }; }
   
       /* Enumerate @{edge->?}s and accumulate diedral angles: */
        
       for (j = 0; j < NE; j++)
       {
         if (@{edge->?}Relevant[j])
         {
           ??? e = @{edge->?}[j];
           {
             ComputeAverageDiedralAngle(e, da[j], zmDbe[j]);
           }   
         }
       }
      

       /* Compute energy "e" from dihedral angles, and the gradient
          "eDvp": */

       for (i = 0; i < NV; i++){ eDc[i] = (r4_t){0.0,..}; }

       e = 0.0;
       for (j = 0; j < NE; j++)
       {
         ??? ee = @{edge->?}[j];
           ??? de = drf[ee->num];
         ??? un = ee.node[0]->num;
         ??? vn = ee.node[1]->num;
         {
           if (@{edge->?}Relevant[j])
           {
             Accum_e_from_ee(da[j],de,e,eDzm);
             if (grad)
             {
               Distribute_eDzm(un, vn, eDzm, zmDbe[j]);
             }
           }
         }
       }
       
      }
    }
  } /* END Eval */

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

{
} HingeEnergy.