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

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

struct CmpAreaEnergy_private_t 
{
  double K;                   /* The energy normalization factor */
  ElemTableRec_t *top;        /* The map elements */
  bool_vec_t vVar;            /* TRUE if node is variable */
  bool_vec_t wallRelevant;    /* TRUE if wall is relevant */
  double_vec_t af;            /* Area of each wall relevant */
  double_vec_t eDaf;          /* (Work) Derivative of Energy rel. to wall area */
};

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

void SELF_DefTop(SELF_T *erg, ElemTableRec_t *top)
  { erg->K = 1.0/((double)top->wall.nel);     
    erg->top = top;
    erg->vVar = bool_vec_new(top->node.nel);
    erg->wallRelevant = bool_vec_new(top->wall.nel);
      
    /* Allocate area tables: */
    erg->af = double_vec_new(top->wall.nel);
    erg->eDaf = double_vec_new(top->wall.nel);
      
    /* 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->wall.nel; i++){ erg->wallRelevant.el[i] = FALSE; }
  } /* END DefTop */
 
void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { /* Decide which wall are relevant to Area Compression energy. A wall is
       relevant iff it exists, and the wall have at least one variable corner. */
    int NV = erg->top->node.nel;
    int NF = erg->top->wall.nel;
    bool_vec_t vVar = erg->vVar^;
    ??? wall = erg->top->wall^;
    ??? wallRelevant = erg->wallRelevant^;
    {
      assert(variable.nel == NV);
      vVar = variable;
      
      /* Find the relevant walls: */
      for (i = 0; i < NF; i++){ wallRelevant.el[i] = FALSE; }
      for (i = 0; i < NF; i++)
      {
        ??? f = wall[i];
          ??? a = f.pa^;
        Node_t u = OrgV(a);
        Node_t v = OrgV(NextE(a));
        Node_t w = OrgV(PrevE(a));
        bool_t vvar = (vVar.el[u->num]) || (vVar.el[v->num]) || (vVar.el[w->num]);
      {
          if ((f->exists) && (vvar))
          {
            assert(u->exists) && (v->exists) && (w->exists);
            wallRelevant.el[i] = TRUE;
          }
          else
          {
            wallRelevant.el[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;
    int NF = erg->top->wall.nel;
    ??? wall = erg->top->wall^;
    ??? wallRelevant = erg->wallRelevant^;
    ??? af = erg->af^;
    ??? eDaf = erg->eDaf^;
    ??? vVar = erg->vVar^;
    ??? K = erg->K;
    ??? A = ((double)erg->area);
  {

      void Accum_a(*u,v,w: uint; VAR area: double)
        /*
          Compute the area of triangle with endpoints nodes (number) u,v,w.
        */
        {
          ??? aa = r4_Sub(c[u], c[v]);
          ??? a = r4_Norm(aa);
          ??? bb = r4_Sub(c[u], c[w]);
          ??? b = r4_Norm(bb);
          ??? cc = r4_Sub(c[v],c[w]);
          ??? c = r4_Norm(cc);
          ??? p = (a+b+c);
          ??? p2a = p - 2.0 * a;
          ??? p2b = p - 2.0 * b;
          ??? p2c = p - 2.0 * c;
          double num == sqrt(p * p2a * p2b * p2c);
          {
            assert(p!=0.0);
            area = num/4.0; 
          }
        } Accum_a;

      void Accum_e_from_a(a: double; double e;
                               VAR VAR etDa : double)
        /*
          Adds to {e} the energy term corresponting to area wall {a} .
        */
        {
          ??? r = a/A;
            ??? s = Math.log(r)*Math.log(r);
          {
            assert(a!=0.0);
            assert(r!=0.0);
            assert(0.0 <= s);
            e = e + K * s;
            if (grad)
            {
              etDa = 2.0 * K * Math.log(r)/a;
            }
            else
            {
              etDa = 0.0;
            }
          }
        } Accum_e_from_a;

      void Distribute_eDa(iu, iv, iw: uint; eDa: double)
        /*
          Accumulates in {eDc} the gradient of {e} relative to the corners
          of the triangle {iu iv iw}, given the derivative {eDa} of {e} relative
          to the triangle's area {a}.
        */
        {
          ??? u = c[iu];
          ??? v = c[iv]; 
          ??? w = c[iw];
          ??? aa = r4_Sub(u,v); 
          ??? a = r4_Norm(aa);
          ??? bb = r4_Sub(u,w); 
          ??? b = r4_Norm(bb);
          ??? cc = r4_Sub(v,w); 
          ??? c = r4_Norm(cc);
          ??? abc = a * b * c;
          ??? bc = abc/a;
          ??? ac = abc/b;
          ??? ab = abc/c;
          ??? p = (a+b+c);
          ??? p2a = p - 2.0 * a;
          ??? p2b = p - 2.0 * b;
          ??? p2c = p - 2.0 * c;
          ??? num = sqrt(p * p2a * p2b * p2c);
          ??? eDv = eDc[iv];
          ??? eDu = eDc[iu];
          ??? eDw == eDc[iw];
          {
            if (p!=0.0)
            {
              ??? p3 = p * p * p;
                ??? p2 = p * p;
              ??? num1 = 4.0*p3-6.0*p2*(p)+8.0*p*(bc+ac+ab)-8.0*abc;
              ??? aDp = (1.0/8.0) * num1 * (1.0/num);
              ??? eDp = eDa * aDp;
              ??? pDcu = r4_Mix(1.0/a,aa,1.0/b,bb);
              ??? pDcv = r4_Mix(-1.0/a,aa,1.0/c,cc);
              ??? pDcw = r4_Mix(-1.0/b,bb,-1.0/c,cc);
              ??? eDcu = r4_Scale(eDp, pDcu);
              ??? eDcv = r4_Scale(eDp, pDcv);
              ??? eDcw = r4_Scale(eDp, pDcw);
              {     
                if (vVar.el[iv])
                {
                  eDv = r4_Add(eDv,eDcv);
                }
                if (vVar.el[iu])
                {
                  eDu = r4_Add(eDu, eDcu);
                }
                if (vVar.el[iw])
                {
                  eDw = r4_Add(eDw, eDcw);
                }
              } 
            }
          }
        } Distribute_eDa;

      { 
        /* Clear area accumulators: */
        for (i = 0; i < NF; i++){ af[i] = 0.0; }

        /* Enumerate walls and accumulate wall areas: */
        for (j = 0; j < NF; j++)
        {
          if (wallRelevant.el[j])
          {
            ??? f = wall[j];
            ??? a = f.pa^;
            Node_t un = OrgV(a)->num;
            Node_t vn = OrgV(NextE(a))->num;
            Node_t wn = OrgV(PrevE(a))->num;
          {
              Accum_a(un,vn,wn,af[j]); 
            }
          }  
        }
        
        /* Compute energy {e} from walls areas, and the gradient {eDaf} */
   
        e = 0.0;
        for (j = 0; j < NF; j++)
        {
          if (wallRelevant.el[j])
          {
            Accum_e_from_a(af[j],e, eDaf[j]);
          }
          else
          {
            eDaf[j] = 0.0;
          }
        }


        /* Now distribute {eDaf} over {eDc}: */
        for (i = 0; i < NV; i++){ eDc[i] = (r4_t){0.0,0.0,0.0,0.0}; }

        if (grad)
        {
          for (j = 0; j < NF; j++)
          {
            if (wallRelevant.el[j])
            {
              ??? f = wall[j];
                ??? a = f.pa^;
              Node_t un = OrgV(a)->num;
              Node_t vn = OrgV(NextE(a))->num;
              Node_t wn = OrgV(PrevE(a))->num;
            {
                Distribute_eDa(un, vn, wn, eDaf[j]);
              }
            }
          }
        }    

      }
    }
  } /* END Eval */

char * SELF_Name(SELF_T *erg)==
  { return 
      "Area(rarea= " & Fmt.Real(erg->area, Fmt.Style.Fix, prec = 3) & ")"
  } /* END Name */