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


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

TYPE
  bool_vec_t == ARRAY OF bool_t;
  
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 */
      polyRelevant: REF bool_vec_t;     /* TRUE if cell is relevant */
      tetra : REF Place_vec_t;   /* Tetrahedra with consistent orientations */
    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->K = 1.0/((double)top->cell.nel);      
    erg->top = top;
    erg->vVar = bool_vec_new(top->node.nel);
    erg->polyRelevant = bool_vec_new(top->cell.nel);  
    /* Collect cells with consistent orientation  */
    erg->tetra = Triangulation.CollectTetrahedra(top);
    /* 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->cell.nel; i++){ erg->polyRelevant[i] = FALSE; }
  } /* END DefTop */
  
void SELF_DefVar(SELF_T *erg, bool_vec_t *variable)
  { /*
      Decide which cells are relevant to "Orientation" energy.
      A cell is relevant iff it has at least one variable corner.
    */
    
    int NV = erg->top->node.nel;
    ??? NP = erg->top->cell.nel;
    bool_vec_t vVar = erg->vVar^;
    ??? t = erg->tetra^;
    ??? polyRelevant = erg->polyRelevant^;
    {
      assert((variable.nel) == NV);
      vVar = variable;
      
      /* Find the relevant cells: */
      for (i = 0; i < NP; i++){ polyRelevant[i] = FALSE; }
      for (i = 0; i < NP; i++)
      {
        assert(PnegP(t[i])->num == i); 
        ??? a = t[i];
        Node_t u = OrgV(a);
        Node_t v = OrgV(NextE(a));
        Node_t w = OrgV(PrevE(a));
        Node_t x = OrgV(PrevE(PrevF(a)));
        Wall_t f1 = PWall(a);
        Wall_t f2 = PWall(PrevF(a));
        Wall_t f3 = PWall(PrevF(NextE(a)));
        Wall_t f4 = PWall(PrevF(PrevE(a)));
        {
          assert(f1 != f2) && (f2 != f3) && (f3 != f4);       
          polyRelevant[i] = vVar.el[u->num]) || (vVar.el[v->num]) || (vVar.el[w->num]) || (vVar.el[x->num];
        }
      }
    }
  } /* END DefVar */

void SELF_Eval(
    SELF_T erg; 
    Coords_t *c; 
    double *e; 
    bool_t grad;
    Gradient *eDc;
 )
  double *eterm,etermDdet; 
  { 
    int NV = erg->top->node.nel;
    ??? NP = erg->top->cell.nel;
    ??? t = erg->tetra^;
    ??? vVar = erg->vVar^;
    ??? polyRelevant = erg->polyRelevant^;
    ??? K = erg->K;
    ??? minDet = erg->minVol * 6.0;
    {
      double ComputeDet(*U, V, W, X: uint)
        /*
          Compute the determinat of the tetrahedron "u v w x" in R^{3}. 
        */
        {
          ??? pu = c[U];
          ??? pv = c[V];
          ??? pw = c[W];
          ??? px = c[X];
          ??? a = (r4_t){pu[0], pu[1], pu[2], 1.0};
          ??? b = (r4_t){pv[0], pv[1], pv[2], 1.0};
          ??? c = (r4_t){pw[0], pw[1], pw[2], 1.0};
          ??? d = (r4_t){px[0], px[1], px[2], 1.0};
          {
            return r4_det(a,b,c,d);
          }
        } ComputeDet;

      void Compute_e_from_det(det: double; double eterm;
                                   VAR VAR etermDdet: double)
        /*
          Returns in "eterm" the energy term corresponding to a cell with
          determinant "det".  Also, if "grad" is true, stores in "etermDdet"
          the derivative of that term.
        */
        {
          if (det >= minDet)
          {
            eterm = 0.0; etermDdet = 0.0
          }
          else
          {
            ??? h = det/minDet-1.0;
            ??? f = h*h;
            {
              eterm = K * f;
              if (grad)
              {
                ??? hDdet = 1.0/minDet;
                ??? fDdet = 2.0 * h * hDdet;
                {
                  etermDdet = K * fDdet;
                }
              }
              else
              {
                etermDdet = 0.0;
              }
            }
          }
        } Compute_e_from_det;
     
      void Distribute_eDdet(
          uint *iu,iv,iw,ix;
          etermDdet: double)
        /*
          Accumulates in "eDc" the gradient of "eterm" relative to the corners
          of the tetrahedron "iu iv iw ix", given the derivative "etermDdet" of
          "eterm" relative to the tetrahedron's determinant "det".
        */
        {
          ??? pu = c[iu];
          ??? pv = c[iv];
          ??? pw = c[iw];
          ??? px = c[ix];
          ??? pu0 = pu[0], pu1 = pu[1], pu2 = pu[2],
          double pv0 = pv[0], pv1 = pv[1], pv2 = pv[2];
          double pw0 = pw[0], pw1 = pw[1], pw2 = pw[2];
          double px0 = px[0], px1 = px[1], px2 = px[2];

          double detwx23 = pw2 - px2;
          double detwx13 = pw1 - px1;
          double detwx12 = pw1 * px2 - px1 * pw2;
          double detwx03 = pw0 - px0;
          double detwx02 = pw0 * px2 - px0 * pw2;
          double detwx01 = pw0 * px1 - px0 * pw1;

          double detvx23 = pv2 - px2;
          double detvx13 = pv1 - px1;
          double detvx12 = pv1 * px2 - px1 * pv2;
          double detvx03 = pv0 - px0;
          double detvx02 = pv0 * px2 - px0 * pv2;
          double detvx01 = pv0 * px1 - px0 * pv1;

          double detvw23 = pv2 - pw2;
          double detvw13 = pv1 - pw1;
          double detvw12 = pv1 * pw2 - pw1 * pv2;
          double detvw03 = pv0 - pw0;
          double detvw02 = pv0 * pw2 - pw0 * pv2;
          double detvw01 = pv0 * pw1 - pw0 * pv1;

          double detDu0 = + pv1 * detwx23 - pv2 * detwx13 + detwx12;
          double detDu1 = - pv0 * detwx23 + pv2 * detwx03 - detwx02;
          double detDu2 = + pv0 * detwx13 - pv1 * detwx03 + detwx01;

          double detDv0 = - pu1 * detwx23 + pu2 * detwx13 - detwx12;
          double detDv1 = + pu0 * detwx23 - pu2 * detwx03 + detwx02;
          double detDv2 = - pu0 * detwx13 + pu1 * detwx03 - detwx01;


          double detDw0 = + pu1 * detvx23 - pu2 * detvx13 + detvx12;
          double detDw1 = - pu0 * detvx23 + pu2 * detvx03 - detvx02;
          double detDw2 = + pu0 * detvx13 - pu1 * detvx03 + detvx01;

          double detDx0 = - pu1 * detvw23 + pu2 * detvw13 - detvw12;
          double detDx1 = + pu0 * detvw23 - pu2 * detvw03 + detvw02;
          double detDx2 = - pu0 * detvw13 + pu1 * detvw03 - detvw01;

          double eDu0 = etermDdet * detDu0;
          double eDu1 = etermDdet * detDu1;
          double eDu2 = etermDdet * detDu2;

          double eDv0 = etermDdet * detDv0;
          double eDv1 = etermDdet * detDv1;
          double eDv2 = etermDdet * detDv2;

          double eDw0 = etermDdet * detDw0;
          double eDw1 = etermDdet * detDw1;
          double eDw2 = etermDdet * detDw2;

          double eDx0 = etermDdet * detDx0;
          double eDx1 = etermDdet * detDx1;
          double eDx2 = etermDdet * detDx2;
          {            
            if (vVar.el[iu])
            {
              eDc[iu,0] = eDc[iu,0] + eDu0;
              eDc[iu,1] = eDc[iu,1] + eDu1;
              eDc[iu,2] = eDc[iu,2] + eDu2;
            }
            if (vVar.el[iv])
            {
              eDc[iv,0] = eDc[iv,0] + eDv0;
              eDc[iv,1] = eDc[iv,1] + eDv1;
              eDc[iv,2] = eDc[iv,2] + eDv2;
            }
            if (vVar.el[iw])
            {
              eDc[iw,0] = eDc[iw,0] + eDw0;
              eDc[iw,1] = eDc[iw,1] + eDw1;
              eDc[iw,2] = eDc[iw,2] + eDw2;
            }
            if (vVar.el[ix])
            {
              eDc[ix,0] = eDc[ix,0] + eDx0;
              eDc[ix,1] = eDc[ix,1] + eDx1;
              eDc[ix,2] = eDc[ix,2] + eDx2;
            }
          }
        } Distribute_eDdet;
              
      {
        e = 0.0;
        /* Clear gradient accumulators */
        for (i = 0; i < NV; i++){ eDc[i] = (r4_t){0.0, ..}; }
        
        /* Enumerate cells and compute cell determinants: */
        for (j = 0; j < NP; j++)
        {
          if (polyRelevant[j])
          {
            ??? a = t[j];
            Node_t un = OrgV(a)->num;
            Node_t vn = OrgV(NextE(a))->num;
            Node_t wn = OrgV(PrevE(a))->num;
            Node_t xn = OrgV(PrevE(PrevF(a)))->num;
            ??? det = ComputeDet(un, vn, wn, xn);
            {
              Compute_e_from_det(det, eterm, etermDdet);
              /*
              fprintf(stderr, Fmt.LongReal(det) & " " & Fmt.LongReal(eterm) \
                 " " & Fmt.LongReal(etermDdet) & "\n");
              VAR c: char *;
              {
                if (det < 0.0){ c = "-" }else{ c= "+"; }
                fprintf(stderr, c);
              }
              */        
              e = e + eterm;
              if (grad)
              {
                Distribute_eDdet(un, vn, wn, xn, etermDdet)
              }
            }  
          }            
        }
        /*fprintf(stderr, "###\n");*/
      }
    }
  } /* END Eval */

PROCEDURE Name(erg: T) : char *==
  { return "Orient(" \
      "minVol = " & Fmt.LongReal(erg->minVol,Fmt.Style.Fix,prec = 6) \
    ")"
  } /* END Name */

{
} OrientationEnergy.