/****************************************************************************/
/* GRAV.C: routines to compute gravity. Public routines: hackgrav().        */
/*                                                                          */
/* Copyright (c) 1993 by Joshua E. Barnes, Honolulu, HI.                    */
/* It's free because it's yours.                                            */
/****************************************************************************/
/* adapted by Bernd Vollmer, CDS, Observatoire de Strasbourg 2004           */
/****************************************************************************/
#include"nrutil.h"
#include "code.h"
 

local void treescan(nodeptr);			/* does force calculation   */
local bool subdivp(cellptr);			/* can cell be accepted?    */
local void gravsub(nodeptr);			/* compute grav interaction */

/*
 * HACKGRAV: evaluate gravitational field on body p; checks to be
 * sure self-interaction was handled correctly if intree is true.
 */

local bodyptr pskip;				/* skip in force evaluation */
local vector pos0;				/* point to evaluate field  */
local real phi0p, phi0b, phi0s;				/* resulting potential      */
local vector acc0;				/* resulting acceleration   */
local bool skipself;				/* self-interaction skipped */
local bool treeincest = FALSE;			/* tree-incest occured      */
local int pcounter, bcounter, scounter;

void hackgrav(bodyptr p, bool intree)
{
    pskip = p;					/* exclude p from f.c.      */
    SETV(pos0, Pos(p));				/* set field point          */
    phi0p=phi0b=phi0s = 0.0;			/* init total potential     */
    CLRV(acc0);                                 /* and total acceleration   */
    n2bterm = nbcterm = 0;			/* count body & cell terms  */
    skipself = TRUE;				/* watch for tree-incest    */
    pcounter = Nparents(p);
    bcounter = Nbrothers(p);
    scounter = Nsons(p);
    treescan((nodeptr) root);			/* scan tree from root	    */
    if (intree && ! skipself) {			/* did tree-incest occur?   */
	if (! scanopt(options, "allow-incest"))	/*   treat as catastrophic? */
	    error("hackgrav: tree-incest detected\n");
	if (! treeincest)			/*   for the first time?    */
	    eprintf("\n[hackgrav: tree-incest detected]\n");
	;treeincest = TRUE;			/*   don't repeat warning   */
    }
    pnumber+=phi0p;
    bnumber+=phi0b;
    snumber+=phi0s;
    Nparents(p)=pcounter;
    Nbrothers(p)=bcounter;
    Nsons(p)=scounter;
}

/*
 * TREESCAN: iterative routine to do force calculation, starting
 * with node q, which is typically the root cell.
 */

local void treescan(nodeptr q)
{
    while (q != NULL) {				/* while not at end of scan */
	if (Type(q) == CELL &&			/*   is node a cell and...  */
	    subdivp((cellptr) q))		/*   too close to accept?   */
	    q = More(q);			/*     follow to next level */
	else {					/*   else accept this term  */
	    if (q == (nodeptr) pskip)		/*     self-interaction?    */
		skipself = TRUE;		/*       then just skip it  */
	    else {				/*     not self-interaction */
		                                /*       so compute gravity */
		if (Type(q) == BODY)
		     gravsub(q);		/*       count body-body    */
		else
		  nbcterm++;            	/*       count body-cell    */
	    }
	    q = Next(q);			/*     follow next link     */
	}
    }
}

/*
 * SUBDIVP: decide if cell q is too close to accept as a single
 * term.  Also sets qmem, dr, and drsq for use by gravsub.
 */

local cellptr qmem;                     	/* data shared with gravsub */
local vector dr;				/* vector from q to pos0    */
local real drsq;				/* squared distance to pos0 */

local bool subdivp(cellptr q)
{
    DISTV1(drsq,Pos(q),pos0);                    /* compute distance         */
    drsq=drsq*drsq;                             /* and find dist squared    */
    qmem = q;					/* remember we know them    */
    return (drsq < Rcrit2(q));			/* apply standard rule      */
}

/*
 * GRAVSUB: compute contribution of node q to gravitational field at
 * point pos0, and add to running totals phi0 and acc0.
 */

local void gravsub(nodeptr q)
{
    real drab ,drab1, phii, mor3, x,y, ddd;
    bool aski;
    vector ai, quaddr,veldiff;
    real dr5inv, phiquad, drquaddr,helph,helpp,helpq,dx;
    bodyptr pq,pf,pg;
    int i,j, check, check1;
    char qwe[14];

    check=check1=0;
    helpp=MAX(Resolution(pskip),Majaxis(pskip));
    helpq=MAX(Resolution(q),Majaxis(q));

    if (Majaxis(pskip) == 0) helpp=Resolution(pskip);
    if (Majaxis(q) == 0) helpq=Resolution(q);
    
    if (helpq > helpp*1.25) {                  /* Parent */
      if (Resolution(q) >= Majaxis(q)) {       /* based on resolution */
	DISTV1(helph,Pos(pskip),Pos(q));
	drab1=Resolution(q);
	if (helph < drab1/2.) {
	  Parents(pskip)[pcounter]=q;
	  pcounter++;
	  if (pcounter >= maxparents) {
	    printf("too many parents \n");
	    exit(0);
	  }
	  phi0p+=1.;
	  check1=1;
	}   
      } else {                                 /* based on source extension */
	dx=Pos(pskip)[0]-Pos(q)[0];
	ddd=acos(sin(Pos(pskip)[1]/60./60./180.*PI)*sin(Pos(q)[1]/60./60./180.*PI)+cos(Pos(pskip)[1]/60./60./180.*PI)*cos(Pos(q)[1]/60./60./180.*PI)*cos(dx/60./60./180.*PI));
	x=(ddd/sin(ddd))*cos(Pos(pskip)[1]/60./60./180.*PI)*sin(dx/60./60./180.*PI);
	y=(ddd/sin(ddd))*(sin(Pos(pskip)[1]/60./60./180.*PI)*cos(Pos(q)[1]/60./60./180.*PI)-cos(Pos(pskip)[1]/60./60./180.*PI)*sin(Pos(q)[1]/60./60./180.*PI)*cos(dx/60./60./180.*PI));
	x=x*60.*60.*180./PI;
	y=y*60.*60.*180./PI;
	phii=(PA(q))/180.*PI;
	mor3=(x*cos(phii)-y*sin(phii))*(x*cos(phii)-y*sin(phii))/(Majaxis(q)/2.*Majaxis(q)/2.)+(x*sin(phii)+y*cos(phii))*(x*sin(phii)+y*cos(phii))/(Minaxis(q)/2.*Minaxis(q)/2.);
	if (mor3 < 1.) {
	  Parents(pskip)[pcounter]=q;
	  pcounter++;
	  if (pcounter >= maxparents) {
	    printf("too many parents \n");
	    exit(0);
	  }
	  phi0p+=1.;
	  check1=1;
	}   
      }	
    }
    else if (helpq >= 0.75*helpp && helpq <= 1.25*helpp) {     /* Brother */
      if (Resolution(pskip) >= Majaxis(pskip)) {    /* based on resolution */
	DISTV1(helph,Pos(pskip),Pos(q));
	drab1=Resolution(pskip);
	if (helph < drab1/2.) {
	  Brothers(pskip)[bcounter]=q;
	  bcounter++;
	  if (bcounter >= maxbrothers) {
	    printf("too many brothers \n");
	    exit(0);
	  }
	  phi0b+=1.;
	  check1=1;
	}   
      } else {                                 /* based on source extension */
	dx=Pos(q)[0]-Pos(pskip)[0];
	ddd=acos(sin(Pos(q)[1]/60./60./180.*PI)*sin(Pos(pskip)[1]/60./60./180.*PI)+cos(Pos(q)[1]/60./60./180.*PI)*cos(Pos(pskip)[1]/60./60./180.*PI)*cos(dx/60./60./180.*PI));
	x=(ddd/sin(ddd))*cos(Pos(q)[1]/60./60./180.*PI)*sin(dx/60./60./180.*PI);
	y=(ddd/sin(ddd))*(sin(Pos(q)[1]/60./60./180.*PI)*cos(Pos(pskip)[1]/60./60./180.*PI)-cos(Pos(q)[1]/60./60./180.*PI)*sin(Pos(pskip)[1]/60./60./180.*PI)*cos(dx/60./60./180.*PI));
	x=x*60.*60.*180./PI;
	y=y*60.*60.*180./PI;
	phii=(PA(pskip))/180.*PI;
	mor3=(x*cos(phii)-y*sin(phii))*(x*cos(phii)-y*sin(phii))/(Majaxis(pskip)/2.*Majaxis(pskip)/2.)+(x*sin(phii)+y*cos(phii))*(x*sin(phii)+y*cos(phii))/(Minaxis(pskip)/2.*Minaxis(pskip)/2.);
	if (mor3 < 1.) {
	  Brothers(pskip)[bcounter]=q;
	  bcounter++;
	  if (bcounter >= maxbrothers) {
	    printf("too many brothers \n");
	    exit(0);
	  }
	  phi0b+=1.;
	  check1=1;
	}   
      }	
    }
    else if (helpq < 0.75*helpp) {                  /* Son */
      if (Resolution(pskip) >= Majaxis(pskip)) {    /* based on resolution */
	DISTV1(helph,Pos(pskip),Pos(q));
	drab1=Resolution(pskip);
	if (helph < drab1/2.) {
	  Sons(pskip)[scounter]=q;
	  scounter++;
	  if (scounter >= maxsons) {
	    printf("too many sons %s %s \n",Name(pskip),Name(q));
	    scounter--;
	    /*exit(0);*/
	  }
	  phi0s+=1.;
	  check1=1;
	}   
      } else {                                 /* based on source extension */
	dx=Pos(q)[0]-Pos(pskip)[0];
	ddd=acos(sin(Pos(q)[1]/60./60./180.*PI)*sin(Pos(pskip)[1]/60./60./180.*PI)+cos(Pos(q)[1]/60./60./180.*PI)*cos(Pos(pskip)[1]/60./60./180.*PI)*cos(dx/60./60./180.*PI));
	x=(ddd/sin(ddd))*cos(Pos(q)[1]/60./60./180.*PI)*sin(dx/60./60./180.*PI);
	y=(ddd/sin(ddd))*(sin(Pos(q)[1]/60./60./180.*PI)*cos(Pos(pskip)[1]/60./60./180.*PI)-cos(Pos(q)[1]/60./60./180.*PI)*sin(Pos(pskip)[1]/60./60./180.*PI)*cos(dx/60./60./180.*PI));
	x=x*60.*60.*180./PI;
	y=y*60.*60.*180./PI;
	phii=(PA(pskip))/180.*PI;
	mor3=(x*cos(phii)-y*sin(phii))*(x*cos(phii)-y*sin(phii))/(Majaxis(pskip)/2.*Majaxis(pskip)/2.)+(x*sin(phii)+y*cos(phii))*(x*sin(phii)+y*cos(phii))/(Minaxis(pskip)/2.*Minaxis(pskip)/2.);
	if (mor3 < 1.) {
	  Sons(pskip)[scounter]=q;
	  scounter++;
	  if (scounter >= maxsons) {
	    printf("too many sons 1 %s %s \n",Name(pskip),Name(q));
	    scounter--;
	  }
	  phi0s+=1.;
	  check1=1;
	}   
      }	
    }
    else {
      printf("error in familiy finding \n");
      exit(0);
    }

}