#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->go.x = g->go.y = 0;

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

    n->gk = d2 > PRECISION2 ? vdiv(d, d2) : vzero();

  }

  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, int fixangle) {

  if (fixangle) {

    g->o = vadd(g->o, vsub(p, phGroupTrans(g, gp)));

    phGroupPlaceNodes(g);

    return;

  }

  Vec d = vsub(p, g->o);

  Vec gd = vsub(gp, g->go);

  double l = vlen(d);

  double gl = vlen(gd);

  if (l > PRECISION) {

    g->o = vsub(p, vmul(d, gl/l));

    if (!(gl > PRECISION)) return;

  } else {

    g->o.x = p.x - gl;

    g->o.y = p.y;

    return;

  }

  g->dx = vunturn(d, vdiv(gd, l*gl));

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

}