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

#include <Octf.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;
  Wall == Triangulation.Wall;
  @{Edge->?} == Triangulation.edge;
  WALLS == Wall_vec_t;
  Cosines == REF double_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 */
      cosine: REF ARRAY OF Cosines;       /* The diedral angles */
      DRF drf;                           /* The Walls Ring Degree for @{edge->?} */
      walls: REF Wall_vec_ts;          /* The walls-ring for @{edge->?} */
      eDcosine: REF ARRAY OF Cosines;     /* (Work) Gradient of "e" rel. to
                                             the dihedral average angle "a".
                                          */
    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->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;    
    erg->cosine = NEW(REF ARRAY OF Cosines, top->NE);
    erg->eDcosine = NEW(REF ARRAY OF Cosines, top->NE);

    for (i = 0; i < top->NE; i++)
    {
      erg->cosine[i] = double_vec_new(erg->drf^[i])
    }

    for (i = 0; i < top->NE; i++)
    {
      erg->eDcosine[i] = double_vec_new(erg->drf^[i])
    }

    /* 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 */

  WALLS *CollectWalls(
     @{Edge->?} *e;
     *ElemTableRec_t *top;
    )==
     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];
             ??? fun = f.node[0]->num;
           ??? fvn = f.node[1]->num;
           ??? fwn = f.node[2]->num;
           ??? eun = e.node[0]->num;
           ??? evn = e.node[1]->num;
           {
             if (((fun == eun) || (fun == evn))){ ct++; }
             if (((fvn == eun) || (fvn == evn))){ ct++; }
             if (((fwn == eun) || (fwn == 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;

void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
      n : uint = 0;
  { /*
      This energy is computed only for edges that exist, have DegreeRingWalls
      three or more, and have only existing walls and 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,pexists,interface,interpoly;
      {
        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];
                ??? a = f1.pa^;
              ??? aPpos = PposP(a);
              ??? aPneg = PnegP(a);
              {
                IF( aPpos!= NULL)) && ((aPneg!=NULL)
                )){
                    interpoly = TRUE;
                  }
                  else
                  {  
                    interpoly = FALSE;
                }
                if (f1->exists
                ){
                    interface = TRUE;
                  }
                  else
                  {
                    interface = FALSE;
                }    
                fexists = f1->exists) && (pexists;
              }
              fexists = TRUE) && (interface;
              pexists = TRUE) && (interpoly;
            }           
                          
            if ((e->exists) && (fexists) && (pexists) && (vvar) && ( de >= 3))
            {
              ??? u = NARROW(u, Triangulation.Node);
              ??? v = NARROW(v, Triangulation.Node);
              {
                assert(u->exists) && (v->exists);
              }
              @{edge->?}Relevant[i] = TRUE;
              n++;
            }
            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^;
    ??? cosine = erg->cosine^;
    ??? K = erg->K;
    ??? drf = erg->drf^;
    ??? walls = erg->walls^;
    ??? eDcosine = erg->eDcosine^;
    ??? vVar = erg->vVar;
    {

      void Compute_cosine(
         *f1,f2: Place_t;
         double *cos;
        )
        /*
          Compute the diedral angle between the walls f1 and f2. 
        */
        {
          ??? a = f1;
          ??? b = f2;
          Node_t ao = OrgV(a)->num;
          Node_t ad = OrgV(NextE(a))->num;
          Node_t ae = OrgV(PrevE(a))->num;
          Node_t bo = OrgV(b)->num;
          Node_t bd = OrgV(NextE(b))->num;
          Node_t be = OrgV(PrevE(b))->num;
          ??? v1a = r4_Sub(c[ad], c[ao]);
          ??? v2a = r4_Sub(c[ae],c[ao]);
          ??? v1b = r4_Sub(c[bd],c[bo]);
          ??? v2b = r4_Sub(c[be],c[bo]);
          ??? p1 = FindOrthogonal(v1a,v2a);
          ??? p2 = FindOrthogonal(v1b,v2b);
          ??? m = r4_Norm(p1);
          ??? n = r4_Norm(p2);
          ??? o = r4_dot(p1,p2);
          ??? d = m * n;
          {
            assert(ao == bo) && (ad == bd);
            cos = o/d;
            /*fprintf(stderr, Fmt.LongReal(cos) & "\n");*/
          }  
        } Compute_cosine;

      r4_t FindOrthogonal(*v1,v2: r4_t)
       /* compute a orthogonal vector to v1, by the Gram-Schmidt
          decomposition.
       */
       {
         ??? u1 = v1;
         ??? v2_u1 = r4_Project(v2, u1);
         ??? u2 = r4_Sub(v2, 2_u1);
         {
           return u2;
         }  
       } FindOrthogonal;
        
      void Accum_e_from_cosine(
        double cos;
        uint d;
        double *eDcos;
       )
        /*
          Adds to "e" the energy term corresponding to dihedral average
          angle "a".
        */
        {
          ??? thetan = 2.0 * FLOAT(Math.Pi, double);
          double theta  = thetan/((double)d);
          double cosideal = Math.cos(theta);
          double sinideal = Math.sin(theta);
          double cos2 = cos * cos;
          double sqr = sqrt(1.0-cos2);
          double first = cosideal * cos;
          double secon = sinideal * sqr;
          double nund = sinideal * cos;
          double firstd = nund/sqr;
          {
            e = e + K * ( 1.0 - 2.0 * (first+secon));
            if (grad)
            {
              eDcos= 2.0 * (firstd - cosideal);
            }
            else
            {
              eDcos = 0.0;
            }
          }
        } Accum_e_from_cosine;

      void Distribute_eDcosine(
        *f1,f2: Place_t;
        double *eDcos;
       )
        {
          ??? a = f1;
          ??? b = f2;
          Node_t ao = OrgV(a)->num;
          Node_t ad = OrgV(NextE(a))->num;
          Node_t ae = OrgV(PrevE(a))->num;
          Node_t be = OrgV(PrevE(b))->num;
          ??? v1a = r4_Sub(c[ad], c[ao]);
          ??? v2a = r4_Sub(c[ae],c[ao]);
          ??? v2b = r4_Sub(c[be],c[ao]);
          ??? p1 = FindOrthogonal(v1a,v2a);
          ??? p2 = FindOrthogonal(v1a,v2b);
          ??? m = r4_Norm(p1);
          ??? n = r4_Norm(p2);
          ??? o = r4_dot(p1,p2);
          ??? d = m * n;    
          ??? q = o/d;
          ??? f1 = r4_Cos(v2a,v1a);
          ??? f2 = r4_Cos(v2b,v1a);
          ??? eDo = eDcos/d;
          ??? eDd = - eDcos * q / d;
          ??? eDm = eDd * n;
          ??? eDn = eDd * m;
          {
            r4_t eDp1 = r4_Mix(eDo, p2, eDm/m, p1);
            r4_t eDp2 = r4_Mix(eDo, p1, eDn/n, p2);
            r4_t eDv1a = r4_Mix(-f1,eDp1,-f2,eDp2);
            r4_t eDv2a = r4_Neg(eDp1);
            r4_t eDv2b = r4_Neg(eDp2);

            if (grad)
            {
              if (vVar.el[ao])
              {
                eDc[ao]= r4_Sub(eDc[ao],r4_Add(r4_Add(eDv1a,eDv2a),eDv2b));
              }
              if (vVar.el[ad])
              {
                eDc[ad] = r4_Add(eDc[ad],eDv1a);
              }
              if (vVar.el[ae])
              {
                eDc[ae] = r4_Add(eDc[ae],eDv2a);
              }
              if (vVar.el[be])
              {
                eDc[be] = r4_Add(eDc[be],eDv2b);
              }
            } 
          }
        } Distribute_eDcosine;

      { 
        /* Clear dihedral angle and their derivative accumulators: */
        for (j = 0; j < NE; j++)
        {
          for (i = 0; i <= (drf[j]-1); i++)
          {
            cosine[j,i] = 0.0;
            eDcosine[j,i] = 0.0;
          }
        }

        for (i = 0; i < NV; i++)
        {
          eDc[i] = (r4_t){0.0,0.0,0.0,0.0};
        }
  
        /* Enumerate @{edge->?}s and accumulate diedral angles: */
        for (j = 0; j < NE; j++)
        {
          if (@{edge->?}Relevant[j])
          {
            ??? e = @{edge->?}[j];
              ??? eun = e.node[0]->num;
            ??? evn = e.node[1]->num;
            ??? fie = walls[e->num];
            {
              VAR ao,Place_t @p;
                  uint i = 0;
              {
                ao = fie[0].pa^;
                a = ao;
                do 
                {
                  Node_t a00 = OrgV(a)->num;
                   ??? a11 = OrgV(Clock(a))->num;
                 Place_t b = NextF(a);
                 {
                    assert((a00 == eun) || (a00 == evn) AND
                              (a11 == eun) || (a11 == evn)
                   );
                    Compute_cosine(a,b,cosine[j,i]);
                    i++;
                    a = b;
                  }
                } while (!( ( a == ao)    ));
              }
            }   
          }            
        }

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

        e = 0.0;
        for (j = 0; j < NE; j++)
        {
          ??? ee = @{edge->?}[j];
            ??? fie = walls[ee->num];
          ??? de = drf[ee->num];
          {
            if (@{edge->?}Relevant[j])
            {
              for (i = 0; i < de; i++)
              {
                ??? f1 = fie[i];
                  ??? af1 = f1.pa^;
                Place_t af2 = NextF(af1);
                {
                  Accum_e_from_cosine(cosine[j,i],de,eDcosine[j,i]);
                  if (grad)
                  {
                    Distribute_eDcosine(af1,af2,eDcosine[j,i]);
                  }
                }
              }
            }
          }
        }
      }
    }
  } /* END Eval */

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

{
} EquaAngleEnergy.