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

    if (!g->fixed)

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

}