#include "phisics.h"

void phLinkUnreg(PhLink *l) {

  if (!l->cell) return;

  l->next->prev = l->prev; l->prev->next = l->next;

  if (l->next == l) *l->cell = NULL; else

    if (*l->cell == l) *l->cell = l->next;

  l->cell = NULL;

  l->prev = l->next = NULL;

}

void phLinkReg(PhLink *l, PhCell *c) {

  if (l->cell == c) return;

  phLinkUnreg(l);

  if (*c) {

    l->next = *c;

    l->prev = l->next->prev;

    l->next->prev = l;

    l->prev->next = l;

  } else {

    *c = l;

    l->next = l->prev = l;

  }

  l->cell = c;

}

void phNodeUnregCell(PhNode *n)

  { for(int i = 0; i < 4; ++i) phLinkUnreg(n->l+i); }

void phNodeRegCell(PhNode *n, PhCell *c, int stride) {

  if (n->l->cell == c) return;

  phLinkReg(n->l+0, c);

  phLinkReg(n->l+1, c+1);

  phLinkReg(n->l+2, c+stride);

  phLinkReg(n->l+3, c+stride+1);

}

void phNodeUpdateCell(PhNode *n) {

  if (n->group && n->group->ph)

    phNodeRegCell(n, physicsCell(n->group->ph, n->p), n->group->ph->hw*2);

  else

    phNodeUnregCell(n);

}

void phNodeUnreg(PhNode *n) {

  if (!n->group) return;

  phNodeUnregCell(n);

  *(n->prev ? &n->prev->next : &n->group->first) = n->next;

  *(n->next ? &n->next->prev : &n->group->last) = n->prev;

  n->prev = n->next = NULL;

  n->group = NULL;

}

void phNodeReg(PhNode *n, PhGroup *g) {

  if (n->group == g) return;

  phNodeUnreg(n);

  for(int i = 0; i < 4; ++i) n->l[i].node = n;

  n->group = g;

  n->prev = g->last;

  *(n->prev ? &n->prev->next : &g->first) = g->last = n;

  phNodeUpdateCell(n);

}

int phNodeBound(PhNode *n, Vec b0, Vec b1) {

  double d;

  int res = 0;

  d = n->p.x - b0.x - n->r; if (d < 0) n->p.x -= d, res = 1;

  d = b1.x - n->p.x - n->r; if (d < 0) n->p.x += d, res = 1;

  d = n->p.y - b0.y - n->r; if (d < 0) n->p.y -= d, res = 1;

  d = b1.y - n->p.y - n->r; if (d < 0) n->p.y += d, res = 1;

  if (res) phNodeUpdateCell(n);

  return res;

}

int phNodeCollision(PhNode *n) {

  if (!n->group || !n->group->ph) return 0;

  PhNode *chain = n;

  unsigned int mark = ++n->group->ph->lastMark;

  for(int i = 0; i < 4; ++i) {

    PhLink *link = n->l+i;

    for(PhLink *l = link->next; l && l != link; l = l->next) {

      PhNode *m = l->node;

      if (m >= n || m->mark == mark) continue;

      m->mark = mark;

      if (n->group == m->group) continue;

      double r = n->r + m->r;

      Vec d = vsub(m->p, n->p);

      double l2 = vlen2(d);

      if (!(l2 < r*r)) continue;

      double l = sqrt(l2);

      d = vmul(d, (r*1.001 - l)/l/2);

      n->p = vsub(n->p, d);

      m->p = vadd(m->p, d);

      m->chain = chain;

      chain = m;

    }

  }

  if (chain == n) return 0;

  int res = 0;

  while(chain) {

    phNodeUpdateCell(chain);

    PhNode *nn = chain->chain;

    chain->chain = NULL;

    chain = nn;

    ++res;

  }

  return res;

}