#include "phisics.h"



void phGroupUpdateCells(PhGroup *g)
  { for(PhNode *n = g->first; n; n = n->next) phNodeUpdateCell(n); }


void phGroupUnreg(PhGroup *g) {
  if (!g->ph) return;
  phGroupUpdateCells(g);
  *(g->prev ? &g->prev->next : &g->ph->first) = g->next;
  *(g->next ? &g->next->prev : &g->ph->last) = g->prev;
  g->prev = g->next = NULL;
  g->ph = NULL;
}


void phGroupReg(PhGroup *g, Physics *ph) {
  if (g->ph == ph) return;
  phGroupUnreg(g);
  g->ph = ph;
  g->prev = ph->last;
  *(g->prev ? &g->prev->next : &ph->first) = ph->last = g;
  phGroupUpdateCells(g);
}


void phGroupRecalc(PhGroup *g) {
  g->cnt = 0;
  g->I = 0;
  g->go = vzero();
  for(PhNode *n = g->first; n; n = n->next)
    { g->go = vadd(g->go, n->gp); ++g->cnt; }
  if (!g->cnt) return;
  g->go = vdiv(g->go, g->cnt);
  
  for(PhNode *n = g->first; n; n = n->next) {
    Vec d = vsub(n->gp, g->go);
    double d2 = vlen2(d);
    g->I += d2;
    n->gk = d2 > PRECISION2 ? vdiv(d, d2) : vzero();
  }
  
  if (!(vlen2(g->dx) > PRECISION2)) g->dx = vec(1, 0); 
}


void phGroupPlaceNodes(PhGroup* g) {
  for(PhNode *n = g->first; n; n = n->next) {
    n->p = phGroupTrans(g, n->gp);
    phNodeUpdateCell(n);
  }
}


void phGroupFix(PhGroup *g, int fixangle) {
  if (!g->cnt) return;

  g->o = vzero();
  for(PhNode *n = g->first; n; n = n->next)
    g->o = vadd(g->o, n->p);
  g->o = vdiv(g->o, g->cnt);
  
  if (!fixangle) {
    Vec dx = {};
    for(PhNode *n = g->first; n; n = n->next)
      dx = vadd(dx, vuntrans(n->p, n->gk, g->o));
    double l = vlen(dx);
    if (l > PRECISION) g->dx = vdiv(dx, l);
  }
  
  phGroupPlaceNodes(g);
}


void phGroupMove(PhGroup *g, Vec gp, Vec p, double rotRatio) {
  if (rotRatio > PRECISION && g->I > PRECISION) {
    Vec d = vsub(gp, g->go);
    Vec dgp = vsub(phGroupUntrans(g, p), gp);
    double da = vdot(vrot90(d), dgp)*g->cnt*rotRatio/g->I;
    g->dx = vrot(g->dx, da);
  }
  g->o = vsub(p, vturn(vsub(gp, g->go), g->dx));
  phGroupPlaceNodes(g);
}


void phGroupMerge(PhGroup *a, PhGroup *b) {
  if (a == b) return;
  
  PhNode *afirst = a->first, *bfirst = b->first;
  if (afirst && bfirst) {
    a->last->next = bfirst;
    b->first->prev = a->last;
    a->last = b->last;
  } else
  if (!afirst) { a->first = bfirst; a->last = b->last; }
  
  b->first = b->last = NULL;

  for(PhNode *n = bfirst; n; n = n->next) {
    n->group = a;
    if (a->ph != b->ph) phNodeUpdateCell(n);
  }
  
  if (!afirst) { a->cnt = b->cnt; a->go = b->go; } else
    if (bfirst) phGroupRecalc(a);
  
  phGroupRecalc(b);
  phGroupUnreg(b);
}