#include "phisics.h"



void physicsAlloc(Physics *ph, int hw, int hh, double cs) {
  ph->cs = maxd(PRECISION, cs);
  ph->hw = hw > 2 ? hw : 2;
  ph->hh = hh > 2 ? hh : 2;
  ph->cells = (PhCell*)alloc(4*ph->hh*ph->hw*ASIZEOF(*ph->cells));
}


void physicsFree(Physics *ph) {
  free(ph->cells);
  memset(ph, 0, sizeof(*ph));
}


PhCell* physicsCell(Physics *ph, Vec p) {
  int cx = (int)floor(p.x/ph->cs + ph->hw - 0.5);
  int cy = (int)floor(p.y/ph->cs + ph->hh - 0.5);
  if (cx > 2*(ph->hw-1)) cx = 2*(ph->hw-1);
  if (cy > 2*(ph->hh-1)) cy = 2*(ph->hh-1);
  if (cx < 0) cx = 0;
  if (cy < 0) cy = 0;
  return ph->cells + 2*ph->hw*cy + cx;
}


void physicsClear(Physics *ph) {
  while(ph->first) {
    while(ph->first->first) phNodeUnreg(ph->first->first);
    phGroupUnreg(ph->first);
  }
}


int physicsUpdate(Physics *ph) {
  int res = 0;
  // collision
  //for(PhGroup *g = ph->first; g; g = g->next)
  //  for(PhNode *n = g->first; n; n = n->next)
  //    res += phNodeCollision(n);
  // bounds
  for(PhGroup *g = ph->first; g; g = g->next)
    for(PhNode *n = g->first; n; n = n->next)
      res += phNodeBound(n, ph->b0, ph->b1);
  // fix
  for(PhGroup *g = ph->first; g; g = g->next)
    phGroupFix(g, ph->fixangle);
  return res;
}


void physicsDrawDebug(Physics *ph) {
  int seed = rand();
  srand(0);
  saveState();
  noFill();
  for(PhGroup *g = ph->first; g; g = g->next) {
    stroke(randColorPtr(g));
    for(PhNode *n = g->first; n; n = n->next)
      circle(n->p.x, n->p.y, n->r);
  }
  restoreState();
  srand(seed);
}