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