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