// clang sph.c blob.c gen.c eigen.c -Wmost -O2
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include "blob.h"
// #include <stdlib.h>
static Pt a2Pt(R m[3]){return (Pt){m[0], m[1], m[2]};} 
static void Pt2a(R a[3], Pt p){a[0]=p.x; a[1]=p.y; a[2]=p.z;}
static R P(Pt w){R tp = 0;
  for(int j = 0; j<SN; ++j) {Pt lv = sub(centr(j), w); tp += ip(lv, nv(j))/len(lv);}
  return (1.5/3)*tp;}
static void matrot(R x[3][3]){for(int k=0; k<VN; ++k) {R m[3], n[3]; Pt2a(n, q[k]);
      for(int i=0; i<3; ++i) {R s=0; for(int j=0; j<3; ++j) s += x[i][j]*n[j]; m[i]=s;}
      q[k] = a2Pt(m);}}
static R mom(char * m){R m1[3], m2[3][3]; R m0 = moments(m1, m2); rm(m, m0, m1, m2);
       if(0) assert(m2[2][2]<m2[0][0] && m2[2][2] < m2[1][1]);
       return m2[0][0]+m2[1][1];}
//static void pts(int j){printf("pts %d\n", j);
  //  for(int i=0; i<VN; ++i) prP("pts", q[i]);}
static void sculpt(Pt nq[VN], Pt q[VN], R om){
  R pt[VN]; for(int j = 0; j<VN; ++j) pt[j]=0; // remember potential at vertices.
  if(1) { // Compute VN potentials (cache savvy)
    int const k = 10; // how many facets to do at a time.
    for(int i = 0; i<SN; i += k) {Pt N[k], c[k]; int lm = i+k<SN?k:SN-i;
      for(int j=0; j<lm; ++j) {N[j] = nv(i+j); c[j] = centr(i+j);}
      for(int i=0; i<VN; ++i) {R p=0; for(int j=0; j<lm; ++j) {
         Pt lv = sub(c[j], q[i]); p += ip(lv, N[j])/len(lv);} pt[i] += p/2;}}}
  else for(int j=0; j<VN; ++j) pt[j]=P(q[j]);
  {R min=100, max=0, a=0; for(int j = 0; j<VN; ++j) {R m =  pt[j];
         if(m<min) min=m; if(m>max) max=m; a+=m;}
    printf("Min = %14.10f, Max = %14.10f, average = %14.10f\n", min, max, a/VN);
  if(0) for(int j=0; j<VN; ++j) {prP("q", q[j]); prP("puff", puff(j));}
  if(0) for(int j=0; j<VN; ++j) nq[j] = add(q[j], sm(0.01*(pt[j]-a), puff(j)));
  else {R const f = 0.0003;
    for(int k=0; k<SN; ++k) {T t = Q[k];
      Pt A = sub(q[t.b], q[t.a]), B = sub(q[t.c], q[t.b]);
      Pt n = sm(f/ip(A, B), cp(A, B)); // unit normal to facet
      nq[t.a] = add(nq[t.a], sm((pt[t.a]-a),n));
      nq[t.b] = add(nq[t.b], sm((pt[t.b]-a),n));
      nq[t.c] = add(nq[t.c], sm((pt[t.c]-a),n));}}}}

static R const AM = 0;  // angular momentum.
int main(){
  R const pi = 4*atan(1), vol = 4*pi/3; // standard volume.
  sph(); mom("first");
  if(1){R x[3][3]= {{1.2, 0.08, .18}, {-0.11, 1.2, 0.2}, {0.13, 0.1, 0.9}}; matrot(x);} // test turn
  mom("second");
  for(int j = 0; j<VN; ++j) q[j] = add(q[j], (Pt){0.1, 0.6, -0.3}); // test move
  mom("third");
  printf("Potential at origin = %e\n", P((Pt){0, 0, 0}));
  for(int cnt=0; cnt<8; ++cnt) {
    R m1[3], m2[3][3]; R m0 = moments(m1, m2); rm("initial", m0, m1, m2); // calc moments
    {Pt m = sm(1/m0, a2Pt(m1)); for(int j=0; j<VN; ++j) q[j] = sub(q[j], m);} // center
    mom("centered");
    for(int j=0; j<3; ++j) for(int k=0; k<3; ++k) m2[j][k] -= m1[j]*m1[k]/m0;
    for(int j=0; j<3; ++j) m1[j]=0;
    {R r[3][3], evals[3]; eigen(m2, evals, r); R const vad = exp(-log(m0/vol)/3);
       for(int i=0; i<3; ++i) for(int j=0; j<3; ++j) r[i][j] *= vad;
//{Pt m; assert(&m.y - &m.x == 1 && &m.z - &m.y == 1);}
// det(a2Pt(&r[0][0]), a2Pt(&r[1][0]), a2Pt(&r[2][0])));
      prP("Eigenvalues", a2Pt(evals));
      for(int j=0; j<3; ++j) {for(int k=0; k<3; ++k) pR(r[j][k]); printf("rot\n");}
      matrot(r);}  // Balance
   {R om = AM/mom("Balanced");    
   if(1) {Pt nq[VN]; for(int j=0; j<VN; ++j) nq[j]=(Pt){0, 0, 0};
           sculpt(nq, q, om); for(int j=0; j<VN; ++j) q[j] = add(q[j], nq[j]);}
   else sculpt(q, q, om);} mom("OOps");}
 return 0;}