#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);

}