text:
/*
Program to trace back particles in a halo to the initial conditions
and/or to print distance of particles from the Halo center
requires gadget file at z=0 and initial conditions
requires center of mass (can be read in from a file created with virialmass.c)

gcc -std=c99 -lm -lgsl -lgslcblas -o trhalo trhalo.c libgad-gcc.o lmfit-2.2/lmmin.o lmfit-2.2/lm_eval.o KdTree.o

gcc -std=c99 -lm -lgsl -lgslcblas -o trhalo trhalo.c -lgad-altix KdTree.o
gcc -std=c99 -lm -lgsl -lgslcblas -o ../bin/altix/trhalo-nopot trhalo.c -lgad-altix-nopot KdTree.o -DNOPOT

stan:
gcc -lm -lgsl -lgslcblas -o ~/bin/trhalo trhalo.c -lgad-stan stan/KdTree.o

*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include "libgad.h"
#include "KdTree.h"
#include "./lmmin.h"
#include "./lm_eval.h"
#define	PI 3.14159265358979323846
#define GAP2BORDER 16000                                     //min distance to boarders to ignore periodic boundaries
#define INCLUDE 0                                    //(friends + INCLUDE*kpc/h) are used to determine CM
#define TRACE_FACTOR 2.0                             //TRACE_FACTOR times virial radius will be traced back to IC
#define h0 0.72
#define VIROD 200
#define DIM 512
#define ENVDENSRAD (4000 * h0)
#define	MIN(a, b)   ((a)<(b)?(a):(b))
#define	MAX(a, b)   ((a)>(b)?(a):(b))
#define ABS(a) ((a) >= 0 ? (a) : -(a))
#define CMP(a,b) ((a)>(b)?(1):(-1))
#define PB(a,b) ((a)>(b)?(a-b):(a))
#define MOVE(a,b) PB(a+b/2,b)
#define MOVEB(a) MOVE((a),boxsz)
#define MV(a,b) ((a)+(b)/2)%(b)
//#define SQR(x) (x)*(x)
#define SOFTENING 0
#define G 6.6742e-11
#define Msun 1.989e30
#define kpc 3.08567758128e19
#define DEBUG 1

#define VA_USE_PART_TYPE 2
#define CM_USE_PART_TYPE 2
#define INERTIA_USE_PART_TYPE 2
#define kNN 50

double cdens;

struct particle {int ind;float dist;};
clock_t t[2];

void usage()
{
  	  fprintf(stderr,"TrHalo v0.05\n");
	  fprintf(stderr," -f <snapshot file> -i <IC file> \n");
	  fprintf(stderr," -m <virialmass file> -n <Halo Number> or -cm <X Y Z>\n");
	  fprintf(stderr,"   if you want to search for CM in an area around the given coordinates\n");
	  fprintf(stderr,"   add -sr <radius> to include particles for the search with distance to cm < radius\n");
	  fprintf(stderr," -gr <gridfile> -tf <tracefactor default 2.0>\n");
	  fprintf(stderr," -jr <print j distribution to file> -cut <create gadget file of the halo>\n");
	  fprintf(stderr," -pb <force periodic boundary conditions (bitwise)>\n");
  	  fprintf(stderr," -n <name of the halo>\n");
  	  fprintf(stderr," -cid <center on particle with a given ID>\n");
  	  fprintf(stderr," -trid <file with particle IDs to trace>\n");
  	  fprintf(stderr," -r <fraction of virial radius for longest axis of Inertia Tensor>\n");
  	  fprintf(stderr," -ui <bitcode particle types for Inertia Tensor>\n");
  	  fprintf(stderr," -gap <min distance to Box-border for PB>\n");
  	  fprintf(stderr," -mcnt <min particle Count for CM Search>\n");
  	  fprintf(stderr," -sd <search distance>\n");

	  exit(1);
}

double fit_fct(double t, double *p)
{
  return log10((p[0]) / ((t/p[1]) * SQR(1+t/p[1])));
}

// void printout ( int n_par, double* par, int m_dat, double* fvec,
//                        void *data, int iflag, int iter, int nfev )
//          {
//            // dummy function to catch fitting output
//          }

float step()
{
  float tm;
  t[1]=clock();
  tm=(float)(t[1]-t[0])/CLOCKS_PER_SEC;
  t[0]=clock();
  return tm;
}

/*
int cmp (struct particle *a, struct particle *b)
{
  if (a[0].dist > b[0].dist) return 1; else return -1;
}
*/
int cmp (const void *first, const void *second)
{
  struct particle *a = (struct particle*) first;
  struct particle *b = (struct particle*) second;
  if (a->dist > b->dist) return 1;
  else if (a->dist < b->dist) return -1;
  else return 0;
}

int cmpind (const void *first, const void *second)
{
  struct particle *a = (struct particle*) first;
  struct particle *b = (struct particle*) second;
  if (a->ind > b->ind) return 1;
  else if (a->ind < b->ind) return -1;
  else return 0;
}

int compare (float *a, float *b)
{
  if (*a > *b) return 1; else return -1;
}

int coord512 (int *result, long index)
{
  int c[3];
  int i;
  result[0]=floor(index%262144/512.0);
  result[1]=(index%512);
  result[2]=floor(index/262144.0);
  for (i=0; i<3; i++)
    if ((result[i]<0) || (result[i]>512)) return 1;
  return 0;
}

int main  (int argc, char *argv[])
{
  typedef float fltarr[3];
  struct particle *part, *vpart;
  gadpart *P;
  struct header ic,evolved, out;
  FILE *fp;
  char vmfile[256],icfile[256],evolvedfile[256],snfile[256],friendsfile[256],idlfile[256],gridfile[256],jrfile[256], gadfile[256], hname[256], parfile[256], idfile[256], datfile[256];
  unsigned int numpart=0, nummass=0;
  int blocksize,i,j,k,l,m,dum,dum2,tr_halo=0,tr_halo_cnt = 0,id,halo,checknpart,mindum,nhalo,count,notrace=1;
  int *tr_halo_id,*tr_halo_i,*tr_halo_i_dum, ca[3], size[3];
  //  fltarr *pos_ev,*pos_ic,*vel, *outpos, *outvel;
  fltarr *pos_ic;
  fltarr shift, diff;
  float *mass_ev, *mass_dum,*mass_ic, *outmass,dist,maxdist,*distance,halomass,halorad, halolmd;
  double posdum,max[3],min[3],srad=0, icmin[3], icmax[3], scm[3];
  double lmd,vcm[3]={0,0,0},jcm[3]={0,0,0},J[3]={0,0,0}, torq[3],rdum[3], vdum[3], massdum;
  int *id_ev, *outid,*iclus, halonpart;
  float tm,linkle,od, tr_factor;
  double mdens,boxsz,cm[3]={0,0,0}, expcm[3], masstot, gridsize, rlog[50], p[50], err[50], d, rad_ratio=0.3;
  short pb[3]={0,0,0};
  int minbox[3], maxbox[3], szbox[3], ***grid, boxind[3], alignf, mincnt=100;
  int nfun, npar, indmax[3], indmin[3], dojr=0, docut=0, vcnt, starsonly=0, dmonly=1, doidl=1, start, end, use_inertia=0, use_va, use_cm, cmset=0, nopb=0, cid=0, traceid=0, gal_all=0, addbh=0, printcm=0;
  double par[2],ddum, dx512, ddum1, totj, envdens, sqrenvdensrad, bgdens, sfl=SOFTENING, meff,  halfmrad, effrad;
  double GAP=GAP2BORDER, searchdist=0;

  t[0]=clock();
  dx512=72000.0/512.0;

  if (DEBUG) printf("DEBUG-Output enabled\n");fflush(stdout);

  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  //filenames are ignored if given as command line parameters in the same order

  strcpy(vmfile,"<none>");
  strcpy(gridfile,"<none>");
  strcpy(parfile,"halodata.txt");
  strcpy(evolvedfile,"");
  strcpy(icfile,"");
  strcpy(idlfile,"<none>");
  tr_factor=TRACE_FACTOR;
  use_inertia=INERTIA_USE_PART_TYPE;
  use_va=VA_USE_PART_TYPE;
  use_cm=CM_USE_PART_TYPE;
  //  tr_halo=123;
  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  i=1;
  if (1==argc) usage();
  while (i<argc)
    {

      if (!strcmp(argv[i],"-f"))
	{
	  i++;
	  strcpy(evolvedfile,argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-i"))
	{
	  i++;
	  strcpy(icfile,argv[i]);
	  i++;
	  notrace=0;
	}
      else if (!strcmp(argv[i],"-m"))
	{
	  i++;
	  strcpy(vmfile,argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-idl"))
	{
	  i++;
	  strcpy(idlfile,argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-trid"))
	{
	  i++;
	  strcpy(idfile,argv[i]);
	  traceid=1;
	  i++;
	}
      else if (!strcmp(argv[i],"-n"))
	{
	  i++;
	  strcpy(hname,argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-gr"))
	{
	  i++;
	  strcpy(gridfile,argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-n"))
	{
	  i++;
	  tr_halo=atoi(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-sr"))
	{
	  i++;
	  srad=atof(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-cid"))
	{
	  i++;
	  cid=atoi(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-gap"))
	{
	  i++;
	  GAP=atof(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-r"))
	{
	  i++;
	  rad_ratio=atof(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-ui"))
	{
	  i++;
	  use_inertia=atoi(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-uva"))
	{
	  i++;
	  use_va=atoi(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-ucm"))
	{
	  i++;
	  use_cm=atoi(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-use"))
	{
	  i++;
	  use_cm=atoi(argv[i]);
	  use_va=use_cm;
	  use_inertia=use_cm;
	  i++;
	}
      else if (!strcmp(argv[i],"-soft"))
	{
	  i++;
	  sfl=atof(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-sd"))
	{
	  i++;
	  searchdist=atof(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-mcnt"))
	{
	  i++;
	  mincnt=atoi(argv[i]);
	  i++;
	}
       else if (!strcmp(argv[i],"-jr"))
	{
	  i++;
	  dojr=1;
	}
       else if (!strcmp(argv[i],"-bh"))
	{
	  i++;
	  addbh=1;
	}
       else if (!strcmp(argv[i],"-noidl"))
	{
	  i++;
	  doidl=0;
	}
      else if (!strcmp(argv[i],"-pcm"))
	{
	  i++;
	  printcm=1;
	}
      else if (!strcmp(argv[i],"-nopb"))
	{
	  i++;
	  nopb=1;
	}
      else if (!strcmp(argv[i],"-ga"))
	{
	  i++;
	  gal_all=1;
	}
      else if (!strcmp(argv[i],"-cut"))
	{
	  i++;
	  docut=1;
	}
      else if (!strcmp(argv[i],"-pb"))
	{
	  i++;
	  dum=atoi(argv[i]);
	  for (j=0; j<3; j++) if (dum&(1<<j)) pb[k]=1;
	  i++;
	}
      else if (!strcmp(argv[i],"-stars"))
	{
	  i++;
	  starsonly=1;
	}
      else if (!strcmp(argv[i],"-all"))
	{
	  i++;
	  dmonly=0;
	}
      else if (!strcmp(argv[i],"-tr"))
	{
	  i++;
          notrace=1;
	}
      else if (!strcmp(argv[i],"-tf"))
	{
	  i++;
	  tr_factor=atof(argv[i]);
	  i++;
	}
      else if (!strcmp(argv[i],"-cm"))
	{
	  i++;
	  cm[0]=atof(argv[i++]);
	  cm[1]=atof(argv[i++]);
	  cm[2]=atof(argv[i++]);
	  cmset=1;
	} else {
	  usage();
	}

    }

  if (strcmp(hname,""))
    {
      sprintf( idlfile, "%s.idl", hname);
      sprintf(gridfile, "%s.gr", hname);
      sprintf( datfile, "%s.dat", hname);
      sprintf(  jrfile, "%s.jr", hname);
      sprintf( gadfile, "%s.gad", hname);
    }

  if (!traceid)
    {

// auxiliary settings for fitting:

    lm_control_type control;
    lm_data_type data;
    lm_initialize_control(&control);

    data.user_func = fit_fct;
    data.user_t = rlog;
    data.user_y = p;
//---------------------------------------------------

    /*
   fp=fopen(evolvedfile,"r");

  fread(&blocksize,sizeof(int),1,fp);
  fread(&evolved,sizeof(struct header),1,fp);                        //read header of evolved gadget file
  fread(&blocksize,sizeof(int),1,fp);
  boxsz=evolved.boxsize;

  for (i=0; i<6; i++)
    {
      numpart+=evolved.npart[i];
      if ((evolved.npart[i]!=0) || (evolved.massarr[i]==0)) nummass+=evolved.npart[i];
    }
  pos_ev =(fltarr *)malloc(sizeof(fltarr)*numpart+addbh);
  vel    =(fltarr *)malloc(sizeof(fltarr)*numpart+addbh);
  id_ev  =(int *)   malloc(sizeof(int)*numpart+addbh);
  mass_dum=(float *) malloc(sizeof(float)*nummass+addbh);
  mass_ev=(float *) malloc(sizeof(float)*numpart+addbh);

  fread(&blocksize,sizeof(int),1,fp);                                //read particle positions of evolved snapshot file
  fread(&P[0].pos,sizeof(fltarr),numpart,fp);
  fread(&blocksize,sizeof(int),1,fp);

  fread(&blocksize,sizeof(int),1,fp);                                //skip velocities
  fread(&P[0].vel,sizeof(fltarr),numpart,fp);
  fread(&blocksize,sizeof(int),1,fp);

  fread(&blocksize,sizeof(int),1,fp);                                //read particle ids of evolved snapshot file
  fread(&P[0].id,sizeof(int),numpart,fp);
  fread(&blocksize,sizeof(int),1,fp);

  if (nummass>0)
    {
  fread(&blocksize,sizeof(int),1,fp);                                //read particle masses of evolved snapshot file
  fread(&mass_dum[0],sizeof(float),nummass,fp);
  fread(&blocksize,sizeof(int),1,fp);
    }

  fclose(fp);
*/
    numpart= readgadget_part(evolvedfile, &evolved, &P);
    if (numpart==0)
	{
	  extern int libgaderr;
	  fprintf(stderr,"LibGad Error Code: %d\n", libgaderr);
	  exit(libgaderr);
	}
  masstot=0;
  j=0;
  for (i=0; i< numpart; i++)
   {
     masstot+=P[i].mass;
     if ((cid) && P[i].id==cid )
       {
	 for (j=0; j<3; j++) cm[j]=P[i].pos[j];
       }
   }
  mdens=masstot/pow(evolved.boxsize*evolved.time,3);

  cdens=(2.78e-8);
  cdens=cdens*(evolved.omegal+evolved.omega0*pow(evolved.time,-3));
  bgdens=cdens*evolved.omega0;
  dum=0;
  for (i=0; i<5; i++)
    {
      dum+=evolved.npart[i];
      if (DEBUG)
      if (evolved.npart[i]>0) printf("%d %f %f\n", i, P[dum-1].mass, P[dum].mass);
    }

  if (DEBUG) printf("readin evolved %.2f\n",step());fflush(stdout);
  printf("snapshot %s\n", evolvedfile);
  printf("ucm %d   ui %d   uva %d\n", use_cm, use_inertia, use_va);
  if (strcmp(vmfile,"<none>")!=0)
    {
      fp=fopen(vmfile,"r");                                         //read virialmassfile
      fscanf(fp,"%d %d %s %s", &checknpart, &nhalo, &snfile, &friendsfile);
      if (strcmp(snfile,evolvedfile)!=0)
	{
	  printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
	  printf("Warning: Evolved Snapshotfile and the one specified in %s are not identical!\n",vmfile);
	  printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
	}
      if (tr_halo > nhalo)
	{
	  fprintf(stderr,"Illegal Halo index\n");
	  exit(2);
	}
      j=1;
      while (j < tr_halo)
	{
	  fscanf(fp,"%d %f %d %f %f %f %f %f", &j, &halomass, &halonpart, &halolmd, &halorad, &cm[0], &cm[1], &cm[2]);
	}
      fclose(fp);
    }

  if ( (!cm[0]) && (!cm[1]) && (!cm[2]) && !(cmset) )
    {
      fprintf(stderr,"Neither -m nor -cm option used\n");
      exit(3);
    }
  if ( (!cm[0]) && (!cm[1]) && (!cm[2]))
    {
      nopb=1;
      //     tr_factor=1.0;
    }
  /*
  printf("n %d\n",j);
  printf("cm %f %f %f\n",cm[0], cm[1], cm[2]);
  printf("friendsfile %s\n",friendsfile);
  printf("snfile %s\n",snfile);
  printf("n %d\n",tr_halo);
  */

  tr_halo_cnt=0;
  masstot=0;

  if (GAP==GAP2BORDER)
    {
      GAP = evolved.boxsize/4.;
    }

  for (k=0; k<3; k++)                                                         //check whether periodic boundaries are needed
    {
      if ( ((cm[k]<GAP) || (cm[k]>(evolved.boxsize-GAP) || pb[k])) && (!nopb))
	{
	  pb[k]=1;
	  cm[k]=MOVE(cm[k],boxsz);
	}
    }
  if (DEBUG) printf("search center \n");fflush(stdout);
  for (i=0; i<3; i++) scm[i]=cm[i];
if (srad>0)
  {
  tr_halo_id=(int *)malloc(numpart*sizeof(int));
  tr_halo_i =(int *)malloc(numpart*sizeof(int));
  maxdist=srad*srad;
  if (dmonly) {start=evolved.npart[0]; end=evolved.npart[0]+evolved.npart[1];}
  else if (starsonly) {start=evolved.npart[0]+evolved.npart[1]+evolved.npart[2]+evolved.npart[3]; end=start+evolved.npart[4];}
  else {start=0; end=numpart;}

  {start=0; end=numpart;}
  if ((end-start)<mincnt) exit(2);
  while (tr_halo_cnt<mincnt)
    {
      tr_halo_cnt=0;
      for (i=start; i < end; i++)
	{
	  dist=0;
	  for (j=0; j<3; j++)
	    if (pb[j]) dist+=pow(MOVE(P[i].pos[j],boxsz)-cm[j],2);
	    else dist+=pow(P[i].pos[j]-cm[j],2);

	  if  ((dist<maxdist) && ( (1<<P[i].type) & use_cm) )
	    {
	      tr_halo_id[tr_halo_cnt]=P[i].id;
	      tr_halo_i[tr_halo_cnt++]=i;
	    }
	}
      maxdist*=4;
    }
  printf("Number of particles for CM calculation: %d\n",tr_halo_cnt );fflush(stdout);
  tr_halo_id=realloc(tr_halo_id,sizeof(int)*tr_halo_cnt);
  tr_halo_i =realloc(tr_halo_i ,sizeof(int)*tr_halo_cnt);

  }

  /*
  for (i=0; i < checknpart; i++)
    {
      dum=1;
      for (k=0, k<3; k++)
	{
	  if (pb[k])
	    {
	      posdum=(MOVE(P[i].pos[k],boxsz));
	      if ( (posdum < (MOVE(cm[k],boxsz)-GAP)) || (posdum > (MOVE(cm[k],boxsz)+GAP )) ) dum=0;
	    } else {
	      posdum=P[i].pos[k];
	      if ( (posdum < (cm[k]-GAP)) || (posdum > (cm[k]+GAP)) ) dum=0;
	    }
	}

       if (dum)
	{
	  tr_halo_id[tr_halo_cnt]=P[i].id;
	  tr_halo_i[tr_halo_cnt++]=i;
	}
    }

  tr_halo_id=realloc(tr_halo_id,sizeof(int)*tr_halo_cnt);
  tr_halo_i =realloc(tr_halo_i ,sizeof(int)*tr_halo_cnt);

  //  for (j=0; j<3; j++) printf("min %f max %f cm %f\n", min[j],max[j],cm[j]);
  //  pos_dum=(fltarr *)malloc(sizeof(fltarr)*numpart);
  tr_halo_i_dum =(int *)malloc(sizeof(int)*numpart);
  count=0;

  for (i=0; i < numpart; i++)                                                //
    {
      dum=1;
      for (j=0; j<3; j++)
	{
	  if (pb[j])
	    {
	      if (((MOVE(P[i].pos[j],boxsz)+INCLUDE) < min[j]) || ((MOVE(P[i].pos[j],boxsz)-INCLUDE) > max[j])) dum=0;
	    }
	    else {if (((P[i].pos[j]+INCLUDE) < min[j]) || ((P[i].pos[j]-INCLUDE) > max[j])) dum=0;}
	}
      if (dum)
	{
	  //	  for (j=0; j<3; j++) pos_dum[count][j]=P[i].pos[j];
	  tr_halo_i_dum[count]=i;
 	  count++;
	}
    }
  maxdist=0;
  for (j=0; j<3; j++)
    {
      //if (pb[j]) maxdist+=pow((MOVE(max[j],boxsz)-MOVE(min[j],boxsz))/2,2);else
       maxdist+=pow((max[j]-min[j])/2,2);
    }
  maxdist=sqrt(maxdist)*2;
  */

if (srad>0)
{
  double cvel[3]={0.,0.,0.};
  maxdist=srad;
  count=tr_halo_cnt;
do
  {
    // printf("Center of Mass for halo %d : %f %f %f particles: %d rad %f\n ", tr_halo, cm[0], cm[1], cm[2],count,maxdist);

    //pos_dum=realloc(pos_dum,sizeof(fltarr)*count);
    //tr_halo_i_dum=realloc(tr_halo_i_dum,sizeof(int)*count);

    //printf("ja\n");fflush(stdout);

  masstot=0;
  maxdist*=0.92;
  for (j=0; j<3; j++){rdum[j]=cm[j];}
  cm[0]=0;
  cm[1]=0;
  cm[2]=0;
  //  printf("%d\n",count);
  for (i=0; i < count; i++)
    {
      for (j=0; j<3; j++)
	{
	  if (pb[j]) cm[j]+=MOVE(P[tr_halo_i[i]].pos[j],boxsz)*P[tr_halo_i[i]].mass;
	  else cm[j]+=P[tr_halo_i[i]].pos[j]*P[tr_halo_i[i]].mass;
	}
      masstot+=P[tr_halo_i[i]].mass;
      //      if (i<10)
      // printf("%f %f %f !\n", P[tr_halo_i[i]].pos[0]*P[tr_halo_i[i]].mass, cm[1], cm[2]);fflush(stdout);
      //printf("%d\n",i);fflush(stdout);
    }

  //  printf("%f %f %f !\n", cm[0], cm[1], cm[2]);fflush(stdout);

  for (j=0; j<3; j++) cm[j]=cm[j]/(masstot);

  dum=0;
  for (i=0; i < count; i++)                                                //
    {
      dist=0;
      for (j=0; j<3; j++)
	{
	  if (pb[j]) dist += pow((MOVE(P[tr_halo_i[i]].pos[j],boxsz)-cm[j]),2);
	  else dist += pow((P[tr_halo_i[i]].pos[j]-cm[j]),2);
	}

      dist=sqrt(dist);
      if (dist < maxdist)
	{
	  //	  for (j=0; j<3; j++) pos_dum[dum][j]=pos_dum[i][j];
	  tr_halo_i[dum]=tr_halo_i[i];
	  dum++;
	}
    }
  count=dum;
  if ((count<50) && (cvel[0]==0))
    {
      masstot=0;
      for (i=0; i< count; i++)
	{
	  for (j=0; j<3; j++)
	    {
	      cvel[j]+=P[tr_halo_i[i]].vel[j]*P[tr_halo_i[i]].mass;
	    }
	  masstot+= P[tr_halo_i[i]].mass;
	}
      for (j=0; j<3; j++) cvel[j]=cvel[j]/(masstot);
      printf("%d particles for cvel-calculation\n",count);
      printf("Central Velocity %10.4f %10.4f %10.4f\n", cvel[0], cvel[1], cvel[2] );
    }
  // printf("count: %d  cm: %f %f %f change: %f% f% f rad: %f \n",count, cm[0],cm[1], cm[2], cm[0]-rdum[0], cm[1]-rdum[1], cm[2]-rdum[2], maxdist);
  } while (count>5);

 if (addbh)
   {
     if (use_cm!=16) printf("Black holed not centered on stars, are you sure? (think about using -ucm 16)\n");
     for (j=0; j<3; j++)
       {
	 P[numpart].pos[j]=cm[j];
	 P[numpart].vel[j]=cvel[j];
       }
     P[numpart].mass=P[0].mass;
     P[numpart].id=numpart+1;
     out=evolved;
     out.npart[5]++;
     out.nall[5]++;
     char outfilename[256];
     sprintf(outfilename,"%s-bh", evolvedfile);
     //     writegadget(outfilename, out, pos_ev, vel, id_ev, mass_ev);
     writegadget_part(outfilename, out, P);
   }

 }
/*
//printf("CM found %.2f now sort by distance and calculate virial radius+mass\n",step());
// free(pos_dum);
 free(tr_halo_i_dum);

part=(struct particle *)malloc(sizeof(struct particle)*tr_halo_cnt);
for (i=0; i< tr_halo_cnt; i++)
       {
	 dist=0;
	 for (j=0; j<3; j++)
	   {
	     if (pb[j]) dist += pow((MOVE(P[tr_halo_i[i]].pos[j],boxsz)-cm[j]),2);
	     else dist += pow((P[tr_halo_i[i]].pos[j]-cm[j]),2);
	   }
	 part[i].dist=sqrt(dist);
	 //part[i].mass=P[tr_halo_i[i]].mass;
	 //part[i].id=P[tr_halo_i[i]].id;
	 part[i].ind=tr_halo_i[i];
       }
 qsort(&part[0],tr_halo_cnt,sizeof(struct particle),(void *)cmp);
 masstot=0;
     i=0;
     od=201;
     while (((od > 200) && (i<tr_halo_cnt)) || (i<11))                        //Add mass until overdensity drops below 200 or all friends are examined
       {
	 masstot+=P[part[i].ind].mass;
	 od=masstot/(pow(part[i].dist*evolved.time,3)*(4.0/3.0)*PI*mdens);
	 //printf("i %d od %f\n",i,od);
	 i++;
       }
     masstot-=P[part[i-1].ind].mass;
  */

  //  if (halorad!=0) maxdist=halorad; else maxdist=1000;

  if (DEBUG) printf("Determine Virial Radius \n");fflush(stdout);
  maxdist=GAP*GAP;
  effrad=30*evolved.hubparam;
  meff=0;
  sqrenvdensrad=SQR(ENVDENSRAD);

     part=(struct particle *)malloc(sizeof(struct particle)*numpart);
     vpart=(struct particle *)malloc(sizeof(struct particle)*numpart);
     count=0;
     envdens=0;
     for (i=0; i< numpart; i++)
       {
	 dist=0;
	 for (j=0; j<3; j++)
	   {
	     if (pb[j]) dist += pow((MOVE(P[i].pos[j],boxsz)-cm[j]),2);
	     else dist += pow((P[i].pos[j]-cm[j]),2);
	     if (dist>maxdist) break;
	   }

	 if (dist < sqrenvdensrad)
	   {
	     envdens+=P[i].mass;
	   }

	 if (dist < maxdist)
	   {
	     dist=sqrt(dist);
	     part[count].dist=dist;
	     part[count++].ind=i;
	     if (dist < effrad)
	       {
		 dum2=0;
		 for (m=0; m<6; m++)
		   {
		     dum2+=evolved.npart[m];
		     if (dum2>i) break;
		   }
		 if ((1<<m) & use_cm)
		   {
		     meff += P[i].mass;
		   }
	       }
	   }

       }
     if (use_cm==1) printf("Gas mass inside 30 kpc %f\n", meff);
     if (use_cm==16) printf("Stellar mass inside 30 kpc %f\n", meff);
     if (use_cm==17) printf("Baryonic mass inside 30 kpc %f\n", meff);
     qsort(&part[0],count,sizeof(struct particle),(void *)cmp);                  //sort particles by distance to CM of halo

     masstot=0;
     i=0;
     od=201;
     double dm_mass=0;
     double b_mass=0;
     double gas_mass=0;
     double star_mass=0;
     while ((od > VIROD) || (i<5))                                                    //Add mass until overdensity drops below 200
       {
	 int ind = part[i].ind;
	 masstot+=P[part[i].ind].mass;
	 if ((P[ind].type == 0) ||(P[ind].type == 4))
	   {
	     b_mass += P[ind].mass;
	     if (P[ind].type == 0) gas_mass += P[ind].mass;
	     else star_mass += P[ind].mass;
	   } else
	   {
	     dm_mass+= P[ind].mass;
	   }
	 od=masstot/(pow(part[i].dist*evolved.time,3)*(4.0/3.0)*PI*cdens);
	 //printf("i %d od %f\n",i,od);
	 vpart[i].dist=part[i].dist;
         vpart[i].ind=part[i].ind;
	 i++;
	 if (i > count)
	   {
	     fp=fopen("error_trhalo.dat","a");
	     fprintf(fp,"Halo %d is making trouble\n",tr_halo);
	     fclose(fp);
	     printf("halo %d is making trouble\n",tr_halo);
	     break;
	   }
	 if (i>numpart) {i--;break;}
	 if ((part[i].dist > searchdist) && (searchdist)) break;
       }
     if (DEBUG) printf("Virial Radius found count %d\n", count);fflush(stdout);
     masstot-=P[part[i-1].ind].mass;
     if ((P[part[i-1].ind].type == 0) ||(P[part[i-1].ind].type == 4))
	   {
	     b_mass -= P[part[i-1].ind].mass;
	   } else
	   {
	     dm_mass-= P[part[i-1].ind].mass;
	   }
     dum=i-1;
     vcnt=i;
     halorad=part[vcnt-1].dist;
     envdens=envdens/((4.0/3.0)*PI*pow(ENVDENSRAD*evolved.time,3));
     vpart=realloc(vpart,sizeof(struct particle)*vcnt);
     qsort(&vpart[0],vcnt,sizeof(struct particle),(void *)cmpind);

     {
       gadpart_dist *tmppart= malloc (sizeof(gadpart_dist) * vcnt);
       for (i=0; i< vcnt; i++)
	 {
	   tmppart[i].dist=part[i].dist;
	   tmppart[i].part=P[part[i].ind];
	 }
       double par[2]={0.005,20};
       double rcs;
       double conc = nfwfit(par, tmppart, vcnt, halorad, sfl, &rcs);
       printf("NFW: %f %f\nc %f\nRs %f\n", par[0], par[1], conc, rcs);
       free(tmppart);
     }

 for (j=0; j<3; j++)
   {
     if (pb[j])
       {
	 cm[j]=MOVE(cm[j],boxsz);
	 printf("!!!periodic boundaries used in Dimension %d !!!\n",j);
       }
   }

 printf("\n halo %5d consists of %5d particles, virial Mass/h %5.2f\n rad %5.2f         od %4.2f\n",tr_halo,vcnt,masstot,part[i-1].dist,od);
 printf(" Baryonic Mass/h %5.2f\n DM Mass/h %5.2f \n\n",b_mass, dm_mass);
 printf("Center of Mass: %10.4f %10.4f %10.4f\n",cm[0]       ,cm[1]       ,cm[2]);
 printf("Shift:          %10.4f %10.4f %10.4f\n",cm[0]-scm[0],cm[1]-scm[1],cm[2]-scm[2]);
 if (printcm)
   {
     FILE *filep= fopen("cm.dat", "w");
     fprintf(filep, "%f %f %f\n",cm[0],cm[1],cm[2]);
     fclose(filep);
   }
 printf("particles in box: %d\n",count);
 printf("Environmental Density (R = %f ): %g\n", ENVDENSRAD,(envdens/bgdens)-1);
 if (DEBUG) printf("... \n");fflush(stdout);
 tr_halo_id=(int *)malloc(count*sizeof(int));
 tr_halo_i =(int *)malloc(count*sizeof(int));
 tr_halo_cnt=0;

 if (docut)
   {
     gadpart *OUT= (gadpart * ) malloc (sizeof(gadpart)*vcnt);
     /*     outpos =(fltarr *)malloc(sizeof(fltarr)*vcnt);
     outvel =(fltarr *)malloc(sizeof(fltarr)*vcnt);
     outid =(int *)malloc(sizeof(int)*vcnt);
     outmass =(float *)malloc(sizeof(float)*vcnt);
     */
     out=evolved;
     for (i=0; i<6; i++)
       {
	 out.npart[i]=0;
	 out.nall[i]=0;
       }
     i=0;
     while (i<vcnt)
       {
	 dist=vpart[i].dist;
	 k=vpart[i].ind;
	 OUT[i]= P[k];
	 for (j=0; j<3; j++)
	   {
	     if (pb[j]) OUT[i].pos[j]=MOVEB(P[k].pos[j])-MOVEB(cm[j]);
	       else OUT[i].pos[j]=P[k].pos[j]-cm[j];
	     //	     OUT[i].vel[j]=P[k].vel[j];
	   }

	 /*
	 for (l=0; l<6; l++)
	   {
 	     dum+=evolved.npart[l];
	     if (k<dum) break;
	   }
	 */
	 l = P[k].type;
	 out.npart[l]++;
	 out.nall[l]++;
	 i++;
       }
     //     writegadget(gadfile, out, outpos, outvel, outid, outmass);
     if (DEBUG) printf("writing... \n");fflush(stdout);
     writegadget_part(gadfile, out, OUT);
     free(OUT);
     /*
     free(outpos);
     free(outvel);
     free(outid);
     free(outmass);
     */
     if (DEBUG) printf("outputfile written... \n");fflush(stdout);
   }

 maxdist=halorad*tr_factor;
 double hmeff=0;
 double galmass=0;
 double gasmass=0;
 double DMmass=0;
 double innertotm=0;
 double gsfr=0;
 double meanage=0;
 int nstars_gal=0;
 dist=0;
 i=0;
 for (j=0; j<50; j++)
   {
     p[j]=0;
     err[j]=0;
     rlog[j]=0;
   }

 masstot=0;
 while ((dist<maxdist) && (i<=numpart))
   {
     dist=part[i].dist;
     dum2=0;
     for (m=0; m<6; m++)
       {
	 dum2+=evolved.npart[m];
	 if (dum2>part[i].ind) break;
       }
     if ((dist<halorad) && (m==1) && (dist>sfl))
       {
	 d=log10(dist);
	 j=floor(d/(log10(halorad)/50));
	 err[j]++;
	 p[j]+=P[part[i].ind].mass;
       }
     if  ( ((1<<m) & use_cm) && (hmeff < (meff/2.0)) )
       {
	 hmeff+=P[part[i].ind].mass;
	 effrad=dist;
       }
     if ((dist <= (halorad * 0.1)) )
       {
	 innertotm+= P[part[i].ind].mass;
	 if (m==4)
	   {
	     galmass+= P[part[i].ind].mass;
	     double redsh = (1/P[part[i].ind].stellarage) - 1;
	     double sage = TIMEDIFF(redsh, (1/evolved.time)-1 );
	     meanage += sage;
	     nstars_gal++;
	   }
	 else if (m==0)
	   {
	     gasmass+= P[part[i].ind].mass;
	     gsfr+= P[part[i].ind].sph->sfr;
	   }
	 else if (m==1) DMmass+= P[part[i].ind].mass;
       }
     i++;
   }
 meanage /= nstars_gal;
 double innerdens=innertotm/(pow((halorad * 0.1)*evolved.time,3)*(4.0/3.0)*PI);
 printf("hmeff: %f effrad: %f\n", hmeff, effrad);
 printf("Galaxy mass (M_star < 10 %% Rvir): %f \n", galmass);
 printf("mean age: %g \n", meanage);
 printf("SFR (< 10 %% Rvir): %g \n", gsfr);
 printf("specific SFR (< 10 %% Rvir): %g \n", gsfr / galmass);
 printf("Gas mass (M_gas < 10 %% Rvir): %f \n", gasmass);
 printf("darkmatter mass (M_gas < 10 %% Rvir): %f \n", DMmass);
 printf("Total mass < 10 %% Rvir: %f \n", innertotm);
 printf("overdensity < 10 %% Rvir: %g\n", innerdens/bgdens - 1);
 printf("density < 10 %% Rvir: %g\n", innerdens);
 if (DEBUG) printf("lambda calculation: \n");fflush(stdout);
  nfun=0;
 k=0;

 /*fitting

 d=log10(halorad)/50;
 for (j=0; j<50; j++)
   {
     if (err[j]!=0)
       {
	 err[nfun]=(1/(sqrt(err[j])));
	 //	 err[nfun]=(1/((err[j])));
	 if (k==0) {
       rlog[nfun]=(pow(10, ((j+1)*d )))/2;
       p[nfun]=p[j]/((4.0/3.0)* PI * (pow(10, ((j+1)*d*3))));
       //       printf("j %d\n", j);

     }
       else {
	 ddum=p[j];
	 rlog[nfun]=(pow(10, ((j+1)*d )) + pow(10, (k*d)))/2;
	 p[nfun]=p[j]/((4.0/3.0)* PI * (pow(10, ((j+1)*d*3)) - pow(10, (k*d*3))));
	 if (p[nfun]<0) printf("%g %e %e \n", ddum, pow(10, ((j+1)*d*3)), pow(10, (k*d*3)) );
       }

//     if (rlog[nfun] < SOFTENING)
//       {
//	 err[nfun]*=10;
//	 printf("S %d\n", nfun);
//       }

     k=j+1;
     nfun++;
       } else {
	 //	 p[j]=0;
	 //	 err[j]=1e10;
       }
   }

 for (j=0; j<nfun; j++)
   {
     //    printf("%2d: rlog %g  p %e err %e d %g d*j %g \n", j, rlog[j], p[j], err[j], pow(10,d*j) ,d*j );
          p[j]=log10(p[j]);

   }

 if (DEBUG) printf("fitting: ");fflush(stdout);
 par[0]=1.00;
 par[1]=10;
 // lm_minimize(nfun, 2, par, err, lm_evaluate_default, printout, &data, &control);
 if (DEBUG) printf("done\n ");fflush(stdout);

 printf("Delta_c %g   Scale Radius %g   Concentration factor c %g\n", par[0], par[1], halorad/par[1]);

  fitting*/

 // printf("%g %g\n", d,  pow(10,d*50));
 i--;
 if (doidl)
   {
     fp=fopen(idlfile,"w");
     fprintf(fp,"dist/mass %s %d %d %g %g %g %g %g\n",vmfile,tr_halo,i,masstot,halorad,cm[0],cm[1],cm[2]);
   }
 dist=0;
 i=0;
 while ((dist < maxdist) && (i<=numpart))
   {
     tr_halo_id[tr_halo_cnt]=P[part[i].ind].id;
     tr_halo_i[tr_halo_cnt++]=part[i].ind;
     dist=part[i].dist;
     if (doidl)
       fprintf(fp,"%g %g %d %d\n",dist, P[part[i].ind].mass, P[part[i].ind].id, P[part[i].ind].type );
     i++;
   }
 if (tr_halo_cnt> numpart) tr_halo_cnt=numpart;
 printf("number of particles within %g times virial radius %d\n trhalo_cnt %d\n", tr_factor ,i, tr_halo_cnt);
 if (doidl) fclose(fp);
 if (count!=tr_halo_cnt)
   {
     tr_halo_id=realloc(tr_halo_id,sizeof(int)*tr_halo_cnt);
     tr_halo_i =realloc(tr_halo_i ,sizeof(int)*tr_halo_cnt);
   }

 if (DEBUG) printf("... \n");fflush(stdout);
 /**********************************************************************************************************************************************/

 // for (l=-5; l<=5; l++)

     count=vcnt; //+l*300;
 for (k=0; k<3; k++)
   {
     jcm[k]=0;
     vcm[k]=0;
     J[k]=0;
   }
 totj=0;
 massdum=0;

 for (j=0; j < count; j++)
   {
     for (k=0; k<3; k++)
       {
	 vcm[k]+=(P[part[j].ind].vel[k]*P[part[j].ind].mass);
	 if (pb[k]) jcm[k]+=(MOVE(P[part[j].ind].pos[k],boxsz)*P[part[j].ind].mass);
 	 else  jcm[k]+=(P[part[j].ind].pos[k]*P[part[j].ind].mass);
       }
      massdum+=P[part[j].ind].mass;
   }
 for (k=0; k<3; k++)
   {
     vcm[k]=vcm[k]/massdum;
     jcm[k]=jcm[k]/massdum;
     jcm[k]=jcm[k];
     //test
     //jcm[k]=cm[k];
   }
 if (dojr) fp=fopen(jrfile,"w");
 for (j=0; j < count; j++)
   {
     for (k=0; k<3; k++)
       {
	 if (pb[k]) rdum[k]=MOVE(P[part[j].ind].pos[k],boxsz)-jcm[k];
	 else rdum[k]=P[part[j].ind].pos[k]-jcm[k];
	 vdum[k]=P[part[j].ind].vel[k]-vcm[k];
       }
     torq[0]=(rdum[1]*vdum[2]-rdum[2]*vdum[1]);
     torq[1]=(rdum[2]*vdum[0]-rdum[0]*vdum[2]);
     torq[2]=(rdum[0]*vdum[1]-rdum[1]*vdum[0]);

     ddum=0;
     for (k=0; k<3; k++)
       {
	 J[k]+=torq[k]*P[part[j].ind].mass;
	 ddum+=torq[k]*torq[k];
       }
     totj+=sqrt(ddum)*P[part[j].ind].mass;

     ddum=0;
     ddum1=0;
     for (k=0; k<3; k++) {ddum+=SQR(rdum[k]); ddum1+=SQR(torq[k]);}
     if (dojr) fprintf(fp,"%g %g\n", sqrt(ddum), sqrt(ddum1)*P[part[j].ind].mass);
    }
 if (dojr) fclose(fp);

 lmd=0;
  for (k=0; k<3; k++) {lmd+=J[k]*J[k];}
  lmd=(sqrt(lmd)*1e10*(Msun/evolved.hubparam)*1e3*(kpc/evolved.hubparam))/(sqrt(2)*part[count-1].dist*(kpc/evolved.hubparam)*massdum*1e10*(Msun/evolved.hubparam));
 // lmd=(totj*1e10*(Msun/evolved.hubparam)*1e3*(kpc/evolved.hubparam))/(sqrt(2)*part[count-1].dist*(kpc/evolved.hubparam)*massdum*1e10*(Msun/evolved.hubparam));
 lmd=lmd/sqrt(G*massdum*1e10*(Msun/evolved.hubparam)/(part[count-1].dist*(kpc/evolved.hubparam)));

 printf("Spin: lambda %e  r %g\n",lmd, part[count-1].dist);

 //   fp=fopen(parfile,"w");
 //   fprintf(fp,"%g %g %g %g \n", lmd, par[0], par[1], halorad/par[1] );
 //   fclose(fp);
 /**********************************************************************************************************************************************/
   //Determine Halo-Shape

   if (use_inertia)
   {
     maxdist=rad_ratio*halorad;
     if (!(use_inertia&2))
       {
	 if (gal_all) maxdist=30*evolved.hubparam;
	 else maxdist=effrad;
       }
     double q=1, s=1, s_old;
     gsl_matrix *I = gsl_matrix_alloc (3, 3);
     gsl_vector *eval = gsl_vector_alloc (3);
     gsl_matrix *evec = gsl_matrix_alloc (3, 3);
     gsl_matrix *LU = gsl_matrix_alloc (3, 3);
     gsl_matrix *inv  = gsl_matrix_alloc (3, 3);
     gsl_eigen_symmv_workspace * w =  gsl_eigen_symmv_alloc (3);
     gsl_matrix *rotation = gsl_matrix_alloc (3, 3);
     gsl_matrix_set_identity(rotation);
     gsl_matrix *resultmatrix = gsl_matrix_alloc (3, 3);
     gsl_matrix_set_zero(I);
     struct gadpart *wpart;

     wpart= (struct gadpart *)malloc(sizeof(struct gadpart)*vcnt);
     m=0;
     for (k=0; k < vcnt; k++)
       {
	 l=part[k].ind;
	 for (j=0; j < 3; j++)
	   {
	     if (pb[j]) wpart[k].pos[j]=MOVEB(P[l].pos[j])-MOVEB(cm[j]);
	       else wpart[k].pos[j]=P[l].pos[j]-cm[j];
	     wpart[k].vel[j]=P[l].vel[j];
	   }
	 wpart[k].mass=P[l].mass;
	 wpart[k].id=P[l].id;
	 dum=0;
	 for (m=0; m<6; m++)
	   {
 	     dum+=evolved.npart[m];
	     if (l<dum) break;
	   }
	 wpart[k].type=m;
       }

     do
     {
     s_old=s;
     gsl_matrix_set_zero(I);
     for (k=0; k < vcnt; k++)
       {
	 l=part[k].ind;
	 for (i=0; i < 3; i++)
	   for (j=i; j < 3; j++)
	     {
	       ddum =wpart[k].pos[i] * wpart[k].pos[j];
	       dist =SQR(wpart[k].pos[0])+SQR(wpart[k].pos[1]/q)+SQR(wpart[k].pos[2]/s);
	       ddum/= dist;
	       if ((sqrt(dist)<maxdist) && ((1<<wpart[k].type)&use_inertia))
		 {
		   gsl_matrix_set(I,i,j, gsl_matrix_get(I,i,j)+ddum);
		   if (i!=j) gsl_matrix_set(I,j,i, gsl_matrix_get(I,j,i)+ddum);
		 }
	     }

       }

     gsl_eigen_symmv (I, eval, evec, w);
     gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_DESC);
     gsl_matrix_memcpy(LU, evec);
     /*
       for (i=0; i < 3; i++)
       {
       for (j=0; j < 3; j++)
       printf("%15.4g", gsl_matrix_get(I,i,j));
       printf("\n");
       }
     */

   for (i=0; i < 3; i++)
     {
       double eval_i  = gsl_vector_get (eval, i);
       //             gsl_vector_view evec_i  = gsl_matrix_column (evec, i);
       if (i==0) ddum = sqrt(eval_i);
       else if (i==1) q=sqrt(eval_i)/ddum;
       else           s=sqrt(eval_i)/ddum;
       //printf ("Ratio = %g\n", sqrt(eval_i)/ddum);
	     //             printf ("eigenvector = \n");
	     //         gsl_vector_fprintf (stdout, &evec_i.vector, "%g");
     }

   gsl_permutation *perm = gsl_permutation_alloc (3);
   int sign;

   gsl_linalg_LU_decomp (LU, perm, &sign);
   gsl_linalg_LU_invert (LU, perm, inv);

   gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
		   1.0, inv, rotation,
		   0.0, resultmatrix);
   gsl_matrix_memcpy (rotation, resultmatrix);

   //Rotate Particles
  gsl_vector *oldpos = gsl_vector_alloc (3);
  gsl_vector *newpos = gsl_vector_alloc (3);
  gsl_vector *oldvel = gsl_vector_alloc (3);
  gsl_vector *newvel = gsl_vector_alloc (3);
  ddum=0;
  for (k=0; k < vcnt; k++)
    {
      for (i=0; i<3; i++)
	{
	  gsl_vector_set(oldpos, i, wpart[k].pos[i]);
	  gsl_vector_set(oldvel, i, wpart[k].vel[i]);
	}
      gsl_blas_dgemv( CblasNoTrans, 1.0, inv, oldpos, 0.0, newpos);
      gsl_blas_dgemv( CblasNoTrans, 1.0, inv, oldvel, 0.0, newvel);
      //ddum += gsl_blas_dnrm2 (oldvel) - gsl_blas_dnrm2 (newvel);
      for (i=0; i<3; i++)
	{
	  wpart[k].pos[i]=gsl_vector_get(newpos, i);
	  wpart[k].vel[i]=gsl_vector_get(newvel, i);
	}
    }
  //printf("%g\n", ddum);
     } while ((ABS(s_old-s)/s) > 1e-2);

     if (docut)
       {
     sprintf( gadfile, "%s_rot", hname);
     FILE *matrixf=fopen(gadfile,"w");
     gsl_matrix_fwrite (matrixf, rotation);
     fclose(matrixf);
       }

     gsl_matrix_set_zero(I);
     for (k=0; k < vcnt; k++)
       {
	 l=part[k].ind;
	 for (i=0; i < 3; i++)
	   for (j=i; j < 3; j++)
	     {
	       ddum =wpart[k].pos[i] * wpart[k].pos[j];
	       dist =SQR(wpart[k].pos[0])+SQR(wpart[k].pos[1]/q)+SQR(wpart[k].pos[2]/s);
	       ddum/= dist;
	       if ((sqrt(dist)<maxdist) && ((1<<wpart[k].type)&use_inertia))
		 {
		   gsl_matrix_set(I,i,j, gsl_matrix_get(I,i,j)+ddum);
		   if (i!=j) gsl_matrix_set(I,j,i, gsl_matrix_get(I,j,i)+ddum);
		 }
	     }

       }
     printf("I: ");
     for (i=0; i < 3; i++) printf("%g ",gsl_matrix_get(I,i,i));
     printf("\n");

     printf("longest axis = %g r_ratio: %g \nq = %g\ns = %g\n", maxdist, rad_ratio, q, s);
     fflush(stdout);
/*************************************************************************************/
/*Calculate Velocity anisotropy                                                      */

     gadpart      * vapart=malloc(vcnt* sizeof(gadpart));
     gadpart_dist * dpart =malloc(vcnt* sizeof(gadpart_dist));
     if ((vapart==NULL) || (dpart==NULL)) {fprintf(stderr, "Error allocating memory!\n");exit(1);}
     double Pi[3]={0.0, 0.0, 0.0};
     double T[3]={0.0, 0.0, 0.0};
     double Pi_PHI=0;
     double  T_PHI=0;
     double Pi_RR=0;
     double  T_RR=0;
     double delta;
     int num_va=0;
     for (i=0; i< vcnt; i++)
       {
	 if ((1<<(wpart[i].type))&use_va)
	   {
	     cpygadpart(&vapart[num_va]    , &wpart[i]);
	     cpygadpart(&dpart[num_va].part, &wpart[i]);
	     dpart[num_va].dist=sqrt(SQR(wpart[i].pos[0])+SQR(wpart[i].pos[1]/q)+SQR(wpart[i].pos[2]/s));
	     num_va++;
	   }
       }
     if (DEBUG) {printf("Begin VA Calculation %d %d\n", vcnt,  num_va);fflush(stdout);}
     if (num_va > 50)
       {

	 qsort(&dpart[0], num_va, sizeof(gadpart_dist), (void *) cmp_dist);
	 if (DEBUG) {printf("particles sorted \n");fflush(stdout);}
	 int distnum= gadsearch(dpart, maxdist, 0, num_va);
	 if (DEBUG) {printf("maxdist determined \n");fflush(stdout);}
	 //	 dpart= realloc(dpart, distnum*sizeof(gadpart_dist));

	 KdNode * root;
	 initKdNode(&root, NULL);
	 buildKdTree(root, vapart, num_va, 0);
	 if (DEBUG) {printf("KD-Tree completed \n");fflush(stdout);}

	 for (i=0; i<3; i++) Pi[j]=0;
	 int incstep;
	 if (distnum<120) incstep=1;
	 else if (distnum<300) incstep=5;
	 else if (distnum<500) incstep=10;
	 else if (distnum<5000) incstep=30;
	 else incstep=50;

	 for (i=0; i<distnum; i+=incstep)
	   {
	     masstot=0;
	     gadpart_dist * knn;
	     double knndist=findkNN(root, &dpart[i].part, 15, &knn, kNN);
	     double meanv[3]={0.0, 0.0, 0.0};
	     double sqrmv[3]={0.0, 0.0, 0.0};
	     double R   = sqrt(SQR(dpart[i].part.pos[0])+SQR(dpart[i].part.pos[1]));
	     double ang;
	     double vR,vR2, vRsum;
	     double vPHI, vPHI2, vPHIsum;
	     for (j=0; j<3; j++)
	       {
		 meanv[j]=dpart[i].part.vel[j]*dpart[i].part.mass;
		 sqrmv[j]=SQR(dpart[i].part.vel[j])*dpart[i].part.mass;
	       }
	     ang    = atan2(dpart[i].part.pos[1], dpart[i].part.pos[0]);
	     vR     = dpart[i].part.vel[0]* cos(ang)+ dpart[i].part.vel[1]* sin(ang);
	     vR2    = SQR(vR)*dpart[i].part.mass;
	     vRsum  = vR*dpart[i].part.mass;
	     vPHI   = -dpart[i].part.vel[0]*sin(ang)+dpart[i].part.vel[1]*cos(ang);
	     vPHI2  = SQR(vPHI)*dpart[i].part.mass;
	     vPHIsum= vPHI*dpart[i].part.mass;

	     masstot=dpart[i].part.mass;
	     for (k=0; k<kNN; k++)
	       {
		 for (j=0; j<3; j++)
		   {
		     meanv[j]+=knn[k].part.vel[j]*knn[k].part.mass;
		     sqrmv[j]+=SQR(knn[k].part.vel[j])*knn[k].part.mass;
		   }
		 //		 ang     = atan2(knn[k].part.pos[1], knn[k].part.pos[1]);
		 ang     = atan2(knn[k].part.pos[1], knn[k].part.pos[0]);
		 vR      = knn[k].part.vel[0]* cos(ang)+ knn[k].part.vel[1]* sin(ang);
		 vR2    += SQR(vR)*knn[k].part.mass;
		 vRsum  += vR*knn[k].part.mass;
		 vPHI    = -knn[k].part.vel[0]*sin(ang)+ knn[k].part.vel[1]* cos(ang);
		 vPHI2  += SQR(vPHI)*knn[k].part.mass;
		 vPHIsum+= vPHI*knn[k].part.mass;
		 masstot+=knn[k].part.mass;
	       }
	     for (j=0; j<3; j++)
	       {
		 meanv[j]=meanv[j]/masstot;
		 sqrmv[j]=sqrmv[j]/masstot;
		 double sigma=(sqrmv[j]-SQR(meanv[j]));
		 Pi[j]+=(sqrmv[j]-SQR(meanv[j]));
		 T[j]+=SQR(meanv[j]);
		 if (sigma < 0) printf("!sigmaerror!%g %g %g %g %g\n",sqrmv[j], meanv[j], SQR(meanv[j]), sigma, masstot);
	       }
	     vR2    = vR2/masstot;
	     vRsum  = vRsum/masstot;
	     vPHI2  = vPHI2/masstot;
	     vPHIsum= vPHIsum/masstot;
	     Pi_RR += vR2-SQR(vRsum);
	      T_RR += SQR(vRsum);
	     Pi_PHI+= vPHI2-SQR(vPHIsum);
	      T_PHI+= SQR(vPHIsum);

	     //      printf("%5d %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g\n", i, dpart[i].dist, knndist, dpart[i].part.pos[0], dpart[i].part.pos[1], dpart[i].part.pos[2], Pi[i][0], Pi[i][1], Pi[i][2]);
	     free(knn);
	   }

	 fprintf(stdout, "PI %g %g %g\n", Pi[0], Pi[1], Pi[2]);
	 double mPi=(Pi[0]+Pi[1])/2;
	 delta= (mPi-Pi[2])/mPi;
	 fprintf(stdout, "Pi_RR  %g\n", Pi_RR);
	 fprintf(stdout, "Pi_PHI %g\n", Pi_PHI);
	 fprintf(stdout, "delta  %g\n", delta);

	 fprintf(stdout, "T  %g %g %g\n",  T[0],  T[1],  T[2]);
	 fprintf(stdout, "T_RR  %g\n", T_RR);
	 fprintf(stdout, "T_PHI %g\n", T_PHI);

	 free(root);
       }
     free(vapart);
     free(dpart);

     if (DEBUG) {printf("End of VA Calculation\n");fflush(stdout);}
/*END VA Calculation************************************************************************************/

/*
  if (docut)
   {
     outpos =(fltarr *)malloc(sizeof(fltarr)*vcnt);
     outvel =(fltarr *)malloc(sizeof(fltarr)*vcnt);
     outid =(int *)malloc(sizeof(int)*vcnt);
     outmass =(float *)malloc(sizeof(float)*vcnt);
     out=evolved;
     qsort(&wpart[0],vcnt,sizeof(struct gadpart),cmp_type);
     for (i=0; i<6; i++)
       {
	 out.npart[i]=0;
	 out.nall[i]=0;
       }
     i=0;
     while (i<vcnt)
       {
	 for (j=0; j<3; j++)
	   {
	     outpos[i][j]=wpart[i].pos[j];
	     outvel[i][j]=wpart[i].P[j].vel;
	   }
	 outid[i]=wpart[i].id;
	 outmass[i]=wpart[i].mass;
	 l=wpart[i].type;
	 out.npart[l]++;
	 out.nall[l]++;
	 i++;
       }
     sprintf( gadfile, "%s_rot.gad", hname);
     writegadget(gadfile, out, outpos, outvel, outid, outmass);
     free(outpos);
     free(outvel);
     free(outid);
     free(outmass);
   }
*/

  gsl_matrix_free(I);
  gsl_matrix_free(evec);
  gsl_matrix_free(LU);
  gsl_matrix_free(inv);
  gsl_matrix_free(rotation);
  gsl_matrix_free(resultmatrix);
  gsl_vector_free(eval);
  gsl_eigen_symmv_free(w);
  free(wpart);

   }

    }else
    {
      fp=fopen(idfile, "r");
      fread(&tr_halo_cnt, sizeof(int), 1,fp);
      fread(cm, sizeof(double), 3,fp);
      tr_halo_id=(int*) malloc(tr_halo_cnt*sizeof(int));
      fread(tr_halo_id, sizeof(int), tr_halo_cnt,fp);
      fclose(fp);
      printf("TraceIDfile (%d particles) read, CM at %f %f %f\n", tr_halo_cnt, cm[0], cm[1], cm[2]);
      fflush(stdout);
    }

 /**********************************************************************************************************************************************/
 if (notrace)
   {
     if (DEBUG)
     printf("no IC-file specified, exitting...\n");
     exit(0);
   }

 i=0;
 numpart=0;
                                                                                //read in IC File
  fp=fopen(icfile,"r");

  fread(&blocksize,sizeof(int),1,fp);
  fread(&ic,sizeof(struct header),1,fp);                        //read header of ic gadget file
  fread(&blocksize,sizeof(int),1,fp);
  boxsz=ic.boxsize;

  for (i=0; i<6; i++) numpart+=ic.npart[i];
  pos_ic =(fltarr *)malloc(sizeof(fltarr)*numpart);
#ifdef LONGIDS
  long *id_ic;
  id_ic  =(long *)   malloc(sizeof(long)*numpart);
#else
  int *id_ic;
  id_ic  =(int *)   malloc(sizeof(int)*numpart);
#endif //LONGIDS

  //  mass_ic=(float *) malloc(sizeof(float)*numpart);

  fread(&blocksize,sizeof(int),1,fp);                                //read particle positions of ic snapshot file
  fread(&pos_ic[0],sizeof(fltarr),numpart,fp);
  fread(&blocksize,sizeof(int),1,fp);

  fread(&blocksize,sizeof(int),1,fp);                                //skip velocities
  fseek(fp,blocksize,SEEK_CUR);
  fread(&blocksize,sizeof(int),1,fp);

  fread(&blocksize,sizeof(int),1,fp);                                //read particle ids of ic snapshot file
#ifdef LONGIDS
  fread(&id_ic[0],sizeof(long),numpart,fp);
#else
  fread(&id_ic[0],sizeof(int),numpart,fp);
#endif //LONGIDS
  fread(&blocksize,sizeof(int),1,fp);

//  fread(&blocksize,sizeof(int),1,fp);                                //read particle masses of ic snapshot file
//  fread(&mass_ic[0],sizeof(float),numpart,fp);
//  fread(&blocksize,sizeof(int),1,fp);
  fclose(fp);

  for (k=0; k<3; k++)                                                         //check whether periodic boundaries are needed
    {
      if ( ((cm[k]<GAP) || (cm[k]>(ic.boxsize-GAP) || pb[k])) && (!nopb))
	{
	  pb[k]=1;
	  //	      cm[k]=MOVE(cm[k],boxsz);
	}
    }

  //  j=P[part[0].ind].id-1;
  j=tr_halo_id[0]-1;
  coord512(ca,j);
  for (k=0; k<3; k++)
    {
      if (pb[k]) {icmin[k]=MV(ca[k], DIM)*dx512; icmax[k]=MV(ca[k], DIM)*dx512;}
      else {icmin[k]=ca[k]*dx512;icmax[k]=ca[k]*dx512;}
    }
  if (!traceid) fp=fopen("ind.dat","w");
  for (k=0; k<3; k++) {indmax[k]=j; indmin[k]=j;}
  for (i=1; i<tr_halo_cnt; i++)
    {
      //      j=P[part[i].ind].id-1;
      j=tr_halo_id[i]-1;
      coord512(ca, j);
      for (k=0; k<3; k++)
	{
	  if (pb[k])
	    {
	      ddum=MV(ca[k], DIM)*dx512;
	      if (ddum < icmin[k]) {icmin[k]= round(ddum); indmin[k]=j;}
	      if (ddum > icmax[k]) {icmax[k]= round(ddum); indmax[k]=j;}
	    } else
	    {
	      ddum=ca[k]*dx512;
	      if (ddum < icmin[k]) {icmin[k]= round(ddum); indmin[k]=j;}
	      if (ddum > icmax[k]) {icmax[k]= round(ddum); indmax[k]=j;}
	    }
	}

      if (!traceid)
      fprintf(fp,"%g %g %g\n",P[tr_halo_i[i]].pos[0]-cm[0],P[tr_halo_i[i]].pos[1]-cm[1],P[tr_halo_i[i]].pos[2]-cm[2]);

    }
  if (!traceid) fclose(fp);

  alignf=2;
  gridsize=72000.0/(64.0*alignf);

  for (k=0; k<3; k++)
	{
	  min[k]=icmin[k];
	  max[k]=icmax[k];
	}

 if (DEBUG)
   {
     printf("indmin: %d %d %d...\n",indmin[0], indmin[1], indmin[2]);fflush(stdout);
     printf("indmax: %d %d %d...\n",indmax[0], indmax[1], indmax[2]);fflush(stdout);
     printf("icmin: %f %f %f...\n",icmin[0], icmin[1], icmin[2]);fflush(stdout);
     printf("icmax: %f %f %f...\n",icmax[0], icmax[1], icmax[2]);fflush(stdout);

}

  if (strcmp(gridfile,"<none>"))              //build a grid and determine which cells contain particles of the halo
  {
  for (k=0; k<3; k++)
    {
      printf("%d\n", pb[k]);
      coord512(ca, indmin[k]);
      minbox[k]= (ca[k]>0) ? (ca[k]-1) : (ca[k]+DIM-1);
      coord512(ca, indmax[k]);
      maxbox[k]= (ca[k]<DIM) ? (ca[k]+1) : (ca[k]-DIM+1);
      if (pb[k]) szbox[k] =ceil((icmax[k]-MOVE(minbox[k]*dx512,boxsz))/(gridsize))+2;
	else szbox[k] =ceil((icmax[k]-(minbox[k]*dx512))/(gridsize))+2;
      size[k]  = (maxbox[k] > minbox[k]) ? (maxbox[k]-minbox[k]) : (maxbox[k]-minbox[k]+DIM);

      //if ((szbox[k]&1)==1) szbox[k]++;
      //      szbox[k]*=alignf;
      printf("%d %d %d \n",minbox[k],maxbox[k],szbox[k]);fflush(stdout);
    }

  grid=  (int ***) malloc (szbox[0]*sizeof(int **));                       //allocate 3-dimensional array

  for (i=0; i < szbox[0]; i++)
    {
      grid[i]=  (int **) malloc (szbox[1] * sizeof(int *));
    }
  for (i=0; i < szbox[0]; i++)
    for (j=0; j < szbox[1]; j++)
      {
	grid[i][j]=   (int *) calloc (szbox[2],sizeof(int ));
      }
  fp = fopen(datfile, "w");
  for (i=0; i<tr_halo_cnt; i++)
    {
      //      j=P[part[i].ind].id-1;
      j=tr_halo_id[i]-1;
      coord512(ca, j);
      for (k=0; k<3; k++)
	{
	  if (pb[k])
	    {
	      boxind[k]=floor(((MV(ca[k],512)*dx512)- MOVE(minbox[k]*dx512, boxsz))/gridsize );
	    } else
	    {
	      boxind[k]=floor((ca[k]*dx512/gridsize)- (minbox[k]*alignf/8.0));
	    }
            fprintf(fp,"%g\t",(ca[k]*dx512/gridsize) );
	  if ((j==indmax[k]) ||(j==indmin[k]))
	    {
	      printf("%d %g %g\n", k, boxind[k]*gridsize , minbox[k]*(gridsize*alignf/8.0));
	      printf("%g %d %d\n", pos_ic[j][k], boxind[k], minbox[k]);fflush(stdout);
	    }
	}
      //      if (DEBUG) {printf("%d %d %d...\n",boxind[0], boxind[1], boxind[2]);fflush(stdout);}
      grid[boxind[0]][boxind[1]][boxind[2]]++;
      fprintf(fp, "%d\t%d\t%d\t%d\t%d\t%d\n", boxind[0], boxind[1], boxind[2] , ca[0], ca[1], ca[2]);
      //      if ((i<100) && (grid[boxind[0]][boxind[1]][boxind[2]]<100000)) grid[boxind[0]][boxind[1]][boxind[2]]+=100000;
    }
    fclose(fp);
    printf("%g\t%g\t%g\n!\n", dx512, gridsize, dx512/gridsize);
  if (DEBUG) {printf("grid built...\n");fflush(stdout);}
  fp=fopen(gridfile,"w");
  blocksize=28;
  fwrite(&blocksize,sizeof(int),1,fp);
  fwrite(&szbox[0],sizeof(int),3,fp);
  fwrite(&minbox[0],sizeof(int),3,fp);
  fwrite(&alignf,sizeof(int),1,fp);
  fwrite(&blocksize,sizeof(int),1,fp);

  blocksize=sizeof(int)*szbox[0]*szbox[1]*szbox[2];
  fwrite(&blocksize,sizeof(int),1,fp);
  for (k=0; k < szbox[2]; k++)
    for (j=0; j < szbox[1]; j++)
	for (i=0; i < szbox[0]; i++)
	fwrite(&grid[i][j][k],sizeof(int),1,fp);
  fwrite(&blocksize,sizeof(int),1,fp);
  fclose(fp);
  }

  printf("Min/Max positions in IC file:\n");fflush(stdout);
  for (k=0; k<3; k++)
    {
      if (pb[k])
	{
	  min[k]=MOVE(min[k],boxsz);max[k]=MOVE(max[k],boxsz);
	  icmin[k]=MOVE(icmin[k], boxsz);icmax[k]=MOVE(icmax[k], boxsz);
	}

      printf("%d:code units: Min %5.2f Max %5.2f physical units: Min %5.2f Max %5.2f\n",k, min[k],max[k],min[k]/ic.hubparam,max[k]/ic.hubparam);
    }
  for (k=0; k<3; k++)
    {
      //printf("%d min %g max %g\n", k, min[k], max[k]);
    }
  printf("minx=%g;\n",icmin[0]);
  printf("maxx=%g;\n",icmax[0]);
  printf("miny=%g;\n",icmin[1]);
  printf("maxy=%g;\n",icmax[1]);
  printf("minz=%g;\n",icmin[2]);
  printf("maxz=%g;\n",icmax[2]);
  printf("offx=%d;\n",minbox[0]);
  printf("offy=%d;\n",minbox[1]);
  printf("offz=%d;\n",minbox[2]);
  printf("sizex=%d;\n",size[0]);
  printf("sizey=%d;\n",size[1]);
  printf("sizez=%d;\n",size[2]);

  printf("Expected CM in resimulation: ");
  for (k=0; k<3; k++)
    {
      diff[k]=icmax[k]-(minbox[k]*dx512);
      diff[k]=(diff[k]>0)? diff[k] : (diff[k]+boxsz);
      shift[k]=( (boxsz/2.0) - (icmin[k]+(diff[k]/2.0)));
      expcm[k]=(cm[k]+shift[k]) > 0 ? (cm[k]+shift[k]) : (cm[k]+shift[k])+boxsz;
      printf(" %g ", expcm[k]);
    }
  printf("\n");

  free(part);
  printf("Time needed: %.2f sec\n",((float)clock())/CLOCKS_PER_SEC);
  return 0;
}


text:
//====---- Sudoku/Options.h                                           ----====//
//
// Data object containing and managing available options.
//====--------------------------------------------------------------------====//
// Templated with element size.
//
// The 0-bit is the inverse answer-bit. if [0]==0 the answer is set.
//
//====--------------------------------------------------------------------====//
#pragma once

#include "Size.h"
#include "Value.h"

#include <gsl/gsl>

#include <bitset>
#include <string>
#include <vector>

#include <utility>

#include <cassert>
#include <cstddef> // size_t
#include <cstdint> // int64_t, uint32

namespace Sudoku
{
template<int E>
class Options
{
	static_assert(E >= 1);
	using bitset = std::bitset<E + 1>;

public:
	Options() noexcept;
	Options(int) = delete; // NOLINT: implicit catch all that convert to bitset
	explicit Options(const bitset&) noexcept; // 0th bit is last in input
	explicit Options(bitset&&) noexcept;
	explicit constexpr Options(Value) noexcept;
	Options& operator=(Value) noexcept;
	Options& operator=(const bitset&) noexcept;
	Options& operator=(bitset&&) noexcept;

	Options& clear() noexcept; // remove all options
	Options& reset() noexcept; // set all options
	Options& flip() noexcept;
	Options& remove_option(Value const&); // remove single option
	// TODO Options& remove_option(Value, ...);	// remove mentioned
	Options& add(Value const&);                  // add single option
	Options& set(Value const&);                  // set to answer
	Options& add_nocheck(Value const&) noexcept; // add single option
	Options& set_nocheck(Value const&) noexcept; // set to answer

	[[nodiscard]] constexpr size_t size() const noexcept;
	[[nodiscard]] size_t count() const noexcept;     // count available options
	[[nodiscard]] size_t count_all() const noexcept; // count all (incl. answer)
	[[nodiscard]] bool all() const noexcept;       // test all options available
	[[nodiscard]] bool test(Value const&) const;   // if an option, or answer
	[[nodiscard]] bool is_answer() const noexcept; // is set to answer
	[[nodiscard]] bool is_empty() const noexcept;

	[[nodiscard]] constexpr bool operator[](Value const&) const noexcept;
	auto operator[](Value const&) noexcept;

	[[nodiscard]] friend bool
		operator==(const Options<E>& left, const Options<E>& right) noexcept
	{
		//? operator== what about the 0th 'is answer' bit?
		return left.data_ == right.data_;
	}
	[[nodiscard]] friend bool
		operator<(const Options<E>& left, const Options<E>& right)
#if defined(__ICL) && __ICL <= 1900
			noexcept(false)
#else
			noexcept(sizeof(Options<E>) <= sizeof(std::uint64_t))
#endif // __ICL
	{
		if constexpr (sizeof(Options<E>) <= sizeof(std::uint32_t))
		{
			return left.data_.to_ulong() < right.data_.to_ulong();
		}
		else
		{
			return left.data_.to_ullong() < right.data_.to_ullong();
		}
	}

	// combine available options
	Options& operator+=(const Options&) noexcept;
	// XOR
	Options& XOR(const Options&) noexcept;
	// remove options
	Options& operator-=(const Options&) noexcept;

	// Debug Use Only, don't depend on it's result
	[[nodiscard]] std::string DebugString() const;

	// Shared options (binary AND)
	// Prefer: shared(left, right)
	[[nodiscard]] friend Options
		operator&(const Options& left, const Options& right) noexcept
	{
		Options tmp{left};
		tmp.data_ &= right.data_;
		return tmp;
	}

private:
	// 0th bit is "need to solve":
	// false if answer has been set = inverse of answer
	bitset data_{};
}; // class Options

//====---- free-functions ------------------------------------------------====//

template<int E>
[[nodiscard]] bool is_answer(Options<E> const&) noexcept;
template<int E>
[[nodiscard]] constexpr bool is_answer_fast(Options<E> const&) noexcept;
template<int E>
[[nodiscard]] bool is_answer(Options<E> const&, Value const&) noexcept;
template<int E>
[[nodiscard]] bool is_option(Options<E> const&, Value const);
template<int E>
[[nodiscard]] Value get_answer(Options<E> const&) noexcept;
template<int N>
[[nodiscard]] Value to_Value(Options<elem_size<N>> const&) noexcept;
template<int E> // NOLINTNEXTLINE(bugprone-exception-escape)
[[nodiscard]] std::vector<Value> available(const Options<E>&) noexcept(true);
template<int E>
[[nodiscard]] Value read_next(const Options<E>&, Value = Value{0}) noexcept;

template<int E>
[[nodiscard]] bool operator!=(Options<E> const&, Options<E> const&) noexcept;

template<int E>
[[nodiscard]] bool operator==(Options<E> const&, Value const&) noexcept;
template<int E>
[[nodiscard]] bool operator==(Value const&, Options<E> const&) noexcept;
template<int E>
[[nodiscard]] bool operator!=(Options<E> const&, Value const&) noexcept;
template<int E>
[[nodiscard]] bool operator!=(Value const&, const Options<E>&) noexcept;

template<int E>
[[nodiscard]] Options<E> XOR(Options<E> const& A, Options<E> const& B) noexcept;

template<int E>
[[nodiscard]] Options<E>
	operator+(Options<E> const&, Options<E> const&) noexcept;
template<int E>
[[nodiscard]] Options<E>
	operator-(Options<E> const&, Options<E> const&) noexcept;

// return shared options
template<int E>
[[nodiscard]] Options<E>
	shared(Options<E> const& A, Options<E> const& B) noexcept;

//====--------------------------------------------------------------------====//

namespace impl
{
	[[nodiscard]] inline constexpr size_t exp2_(size_t value) noexcept
	{
		return (value < 1U) ? 1U : (2U * exp2_(--value));
	}
	static_assert(exp2_(0U) == 0x1U);
	static_assert(exp2_(1U) == 0x2U);
	static_assert(exp2_(2U) == 0x4U); //-V112
	// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
	static_assert(exp2_(3U) == 0x8U); // NOLINT(readability-magic-numbers)
	// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
	static_assert(exp2_(4U) == 0x10U); // NOLINT(readability-magic-numbers)
	// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
	static_assert(exp2_(5U) == 0x20U); // NOLINT(readability-magic-numbers)
	// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
	static_assert(exp2_(6U) == 0x40U); // NOLINT(readability-magic-numbers)
	// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
	static_assert(exp2_(7U) == 0x80U); // NOLINT(readability-magic-numbers)
	// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
	static_assert(exp2_(8U) == 0x100U); // NOLINT(readability-magic-numbers)
	// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
	static_assert(exp2_(9U) == 0x200U); // NOLINT(readability-magic-numbers)

	// generate a bit-mask to use a unique bit per value
	//   empty for Value outside domain.
	template<int E>
	[[nodiscard]] inline constexpr size_t exp2_(Value value) noexcept
	{
		return (value > Value{E}) ? 0U : exp2_(static_cast<size_t>(value));
	}
	// generate a bit-mask to set all bits in std::bitset
	[[nodiscard]] inline constexpr size_t all_set(size_t elements) noexcept
	{
		return (exp2_(++elements) - 1U);
	}
} // namespace impl

//====--------------------------------------------------------------------====//

//	construct with all options set
template<int E>
inline Options<E>::Options() noexcept
{
	data_.flip();
}

template<int E>
inline Options<E>::Options(const bitset& other) noexcept : data_(other)
{ // empty constructor
}

template<int E>
inline Options<E>::Options(bitset&& other) noexcept : data_{other}
{ // empty constructor
}

//	construct with single option set to answer
template<int E>
inline constexpr Options<E>::Options(Value value) noexcept
	: data_{impl::exp2_<E>(value)}
{
	assert(value <= Value{E});
}

//	set to answer value
template<int E>
inline Options<E>& Options<E>::operator=(Value value) noexcept
{
	assert(value <= Value{E});

	data_ = impl::exp2_<E>(value);
	return *this;
}

template<int E>
inline Options<E>& Options<E>::operator=(const bitset& other) noexcept
{
	data_ = other;
	return *this;
}

template<int E>
inline Options<E>& Options<E>::operator=(bitset&& other) noexcept
{
	std::swap(data_, other); // use <utility>
	return *this;
}

//	remove all options
template<int E>
inline Options<E>& Options<E>::clear() noexcept
{
	data_.reset(); // all false
	//???	Note: answer-bit unset too -> answered?
	return *this;
}

//	set all options
template<int E>
inline Options<E>& Options<E>::reset() noexcept
{
	data_.set(); // all true, default starting state
	return *this;
}

template<int E>
inline Options<E>& Options<E>::flip() noexcept
{
	// retain answer flag value
	data_[0].flip(); // noexcept

	data_.flip(); // noexcept
	return *this;
}

//	remove single option
template<int E>
inline Options<E>& Options<E>::remove_option(Value const& value)
{
	assert(is_valid_option<E>(value));
	assert(not Sudoku::is_answer(*this, value));

	data_.set(static_cast<size_t>(value), false);
	return *this;
}

//	add single option
template<int E>
inline Options<E>& Options<E>::add(Value const& value)
{
	assert(is_valid_option<E>(value));
	data_.set(static_cast<size_t>(value), true);
	return *this;
}

//	add single option
template<int E>
inline Options<E>& Options<E>::add_nocheck(Value const& value) noexcept
{
	assert(is_valid_option<E>(value));
	data_[static_cast<size_t>(value)] = true;
	return *this;
}

//	set to answer
template<int E>
inline Options<E>& Options<E>::set(Value const& value)
{
	clear();
	return add(value); // if 0: -> not answer = [0] = true
}

//	set to answer
template<int E>
inline Options<E>& Options<E>::set_nocheck(Value const& value) noexcept
{
	clear();
	return add_nocheck(value);
}

template<int E>
inline constexpr size_t Options<E>::size() const noexcept
{
	return data_.size(); // bits

	//??? size() -1 better?
	// The current implementation works with size() being 1 past the last
	// element. But this allows for size()-1 options to be stored. The
	// direct value to location implementation is convenient. The 0th
	// element in this implementation is just a flag (more could be added).
}

//	available options
//	if 1, a not processed answer
template<int E>
inline size_t Options<E>::count() const noexcept
{
	if (data_[0])
	{
		return data_.count() - 1U;
	}
	return 0; // NO protection vs incorrect answer bit
}

//	(!) counts options, including set answers == ignores answer-bit
//	Returns 1 if {1 option, set as answer} || {1 option, not set as answer}
//	Returns 0 if {empty} || {no option, but not set as answer}
template<int E>
inline size_t Options<E>::count_all() const noexcept
{
	if (data_[0])
	{
		return data_.count() - 1U;
	}
	return data_.count();
}

// Test if all bits are set
template<int E>
inline bool Options<E>::all() const noexcept
{
	return data_.all();
}

//	if an option, or the answer
template<int E>
inline bool Options<E>::test(Value const& value) const
{
	return data_.test(static_cast<size_t>(value));
}

//	check if set to answer
template<int E>
inline bool Options<E>::is_answer() const noexcept
{
	return !data_[0] && data_.count() == 1;
}

// check if set to answer
template<int E>
inline bool is_answer(const Options<E>& options) noexcept
{ // convenience function
	return options.is_answer();
}

// check if set to answer without validation
template<int E>
inline constexpr bool is_answer_fast(const Options<E>& options) noexcept
{
	return !options[Value{0}];
}

// check if set to answer value
template<int E>
inline bool is_answer(Options<E> const& options, Value const& value) noexcept
{
	return options == Options<E>{value};
}

// check if option available
template<int E>
inline bool is_option(const Options<E>& options, const Value value)
{
	assert(is_valid_option<E>(value));
	return (options.test(value) && not is_answer_fast(options));
}

// Test if no options or answers available
template<int E>
inline bool Options<E>::is_empty() const noexcept
{
	return data_.none();
}

// determine the answer value, even if not marked
//   use with is_answer[_fast]() to determine if flagged as answer
template<int E>
inline Value get_answer(const Options<E>& options) noexcept
{
	if (options.count_all() == 1)
	{
		return read_next(options);
	}
	return Value{0};
}

template<int N>
inline Value to_Value(Options<elem_size<N>> const& options) noexcept
{ // specialization of to_Value
	return get_answer(options);
}

// all available options
template<int E> // NOLINTNEXTLINE(bugprone-exception-escape)
inline std::vector<Value> available(Options<E> const& options) noexcept(true)
{ // noexcept: only allocation can throw. Terminate, all is lost anyway.
	const size_t count = options.count();
	std::vector<Value> values{};
	values.reserve(count);

	if (not is_answer(options) && not options.is_empty())
	{
		Value item{0};
		for (size_t i{0}; i < count; ++i)
		{
			item = read_next(options, item);
			values.emplace_back(item);
		}
	}
	return values;
}

//	no-check access read only
template<int E>
inline constexpr bool Options<E>::operator[](Value const& value) const noexcept
{
	assert(value <= Value{E});
	return data_[static_cast<size_t>(value)];
}

//	no-check access
template<int E>
inline auto Options<E>::operator[](Value const& value) noexcept
{
	assert(value <= Value{E});
	return data_[static_cast<size_t>(value)];
}

template<int E>
inline bool operator!=(Options<E> const& left, Options<E> const& right) noexcept
{
	return !(left == right);
}

// short for is_answer(value)
template<int E>
inline bool operator==(Options<E> const& left, Value const& value) noexcept
{
	return is_answer(left, value);
}
template<int E>
inline bool operator==(Value const& value, Options<E> const& right) noexcept
{
	return is_answer(right, value);
}
template<int E>
inline bool operator!=(Options<E> const& left, Value const& value) noexcept
{
	return not(is_answer(left, value));
}
template<int E>
inline bool operator!=(Value const& value, Options<E> const& right) noexcept
{
	return not(is_answer(right, value));
}

//	Combine available options (binary OR)
template<int E>
inline Options<E>& Options<E>::operator+=(const Options& other) noexcept
{
	data_ |= (other.data_);
	return *this;
}
//	Combine (binary OR)
template<int E>
inline Options<E>
	operator+(const Options<E>& left, const Options<E>& right) noexcept
{
	Options<E> tmp{left};
	return tmp += right;
}

//	Per element (binary) XOR, exclusive OR
template<int E>
inline Options<E>& Options<E>::XOR(const Options& other) noexcept
{
	data_ ^= other.data_;
	return *this;
}

// Exclusive OR
template<int E>
inline Options<E> XOR(const Options<E>& A, const Options<E>& B) noexcept
{
	Options<E> tmp{A};
	return tmp.XOR(B);
}

// Shared options
template<int E>
inline Options<E> shared(const Options<E>& A, const Options<E>& B) noexcept
{
	return A & B;
}

// Remove options existing also in other
template<int E>
inline Options<E>& Options<E>::operator-=(const Options& other) noexcept
{
	assert(is_answer_fast(other)); // do not remove the answer-bit
	const Options tmp = ::Sudoku::XOR(*this, other);
	data_ &= tmp.data_;
	return *this;
}

// Only in left
template<int E>
inline Options<E>
	operator-(const Options<E>& left, const Options<E>& right) noexcept
{
	Options<E> tmp{left};
	return tmp -= right;
}

template<int E>
#ifndef fwkUnitTest
[[deprecated("Debug use only")]]
#endif
inline std::string
	Options<E>::DebugString() const
{
	return data_.to_string();
}

// return next option in data
template<int E>
inline Value read_next(const Options<E>& options, Value value) noexcept
{ // default value start = 0
	++value;
	for (; value <= Value{E}; ++value)
	{
		if (options[value])
		{
			return value;
		}
	}
	return Value{0};
}

} // namespace Sudoku

ext: h

max_stars_repo_name: FeodorFitsner/fwkSudoku

max_issues_repo_name: FeodorFitsner/fwkSudoku

max_forks_repo_name: FeodorFitsner/fwkSudoku


text:
static char help[] = "DAE system solver example using TS.  Models two\n"
"equal-mass balls (particles) moving in the plane with a rigid connecting\n"
"rod between.  (Alternatively, -rod_free removes the rod and the motion\n"
"is free and independent.)  A stabilized index-2 constrained Lagrangian\n"
"dynamics formulation is used.  We use cartesian coordinates (x,y) and\n" "velocities (v=dx/dt,w=dy/dt). The system has dimension 10.\n\n";

// DEBUG possibly a good solution using BDF3 and -snes_fd
// ./rod -ts_type bdf -ksp_type preonly -pc_type svd -ts_monitor binary:t.dat -ts_monitor_solution binary:u.dat -ts_dt 0.01 -ts_max_time 1.0 -snes_fd -ts_bdf_order 3
// ./trajectory.py -o figure.png t.dat u.dat

// DEBUG check Jacobian for rod problem using one backward-Euler step
// (2nd case with -snes_fd):
// ./rod -ts_type beuler -ts_max_time 0.1 -ts_dt 0.1 -ksp_type preonly -pc_type svd -snes_monitor -ksp_view_mat
// ./rod -ts_type beuler -ts_max_time 0.1 -ts_dt 0.1 -ksp_type preonly -pc_type svd -snes_monitor -ksp_view_mat -snes_fd

// DEBUG check BDF2 convergence in free problem using Lagrangian formulation:
//for T in 2 3 4 5 7 8 9 10 11 12; do ./rod -rod_free -ts_type bdf -ts_rtol 1.0e-$T -ts_atol 1.0e-$T; done

#include <petsc.h>

typedef struct {
    PetscBool    free;
    PetscReal    g,     // m s-2;  acceleration of gravity
                 m,     // kg;     ball mass
                 l;     // m;      spring or rod length
} RodCtx;

extern PetscErrorCode SetInitial(Vec, RodCtx*);
extern PetscErrorCode FreeExact(Vec, PetscReal, Vec, RodCtx*);
extern PetscErrorCode LagrangeIFcn(TS, PetscReal, Vec, Vec, Vec, void*);
extern PetscErrorCode LagrangeIJac(TS, PetscReal, Vec, Vec, PetscReal,
                                   Mat, Mat, void*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    PetscInt       steps;
    PetscReal      t0 = 0.0, tf = 2.0, dt = 0.1, errnorm;
    Vec            u, u0, uexact;
    Mat            A;
    TS             ts;
    RodCtx         user;
    char           probstr[20] = "rod";

    ierr = PetscInitialize(&argc, &argv, NULL, help); if (ierr) return ierr;

    user.free = PETSC_FALSE;
    user.g = 9.81;
    user.m = 58.0e-3; // 58 g for a tennis ball
    user.l = 0.5;     // rod length, and determines initial condition
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "rod_", "options for rod",
                             ""); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-free","remove rod for free motion",
                            "rod.c", user.free, &user.free,
                            NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-g", "acceleration of gravity (m s-2)",
                            "rod.c", user.g, &user.g, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-l", "rod length (m)", "rod.c",
                            user.l, &user.l, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-m", "mass of each ball (kg)", "rod.c",
                            user.m, &user.m, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    ierr = VecCreate(PETSC_COMM_WORLD,&u); CHKERRQ(ierr);
    ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
    ierr = TSSetApplicationContext(ts,&user); CHKERRQ(ierr);
    ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);

    // set time axis
    ierr = TSSetTime(ts,t0); CHKERRQ(ierr);
    ierr = TSSetMaxTime(ts,tf); CHKERRQ(ierr);
    ierr = TSSetTimeStep(ts,dt); CHKERRQ(ierr);
    ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);

    // set up dimension, equation type, and solver type
    ierr = VecSetSizes(u,PETSC_DECIDE,10); CHKERRQ(ierr);
    ierr = MatCreate(PETSC_COMM_WORLD,&A); CHKERRQ(ierr);
    ierr = MatSetSizes(A,PETSC_DECIDE,PETSC_DECIDE,10,10); CHKERRQ(ierr);
    ierr = MatSetFromOptions(A); CHKERRQ(ierr);
    ierr = MatSetUp(A); CHKERRQ(ierr);
    if (user.free) {
        ierr = TSSetEquationType(ts, TS_EQ_DAE_IMPLICIT_INDEX1);
            CHKERRQ(ierr);
    } else {
        ierr = TSSetEquationType(ts, TS_EQ_DAE_IMPLICIT_INDEX2);
            CHKERRQ(ierr);
    }
    ierr = TSSetIFunction(ts,NULL,LagrangeIFcn,&user); CHKERRQ(ierr);
    ierr = TSSetIJacobian(ts,A,A,LagrangeIJac,&user);CHKERRQ(ierr);
    ierr = TSSetType(ts,TSBEULER); CHKERRQ(ierr); // FIXME BDF3 as default?

    // set-up of u, ts is complete
    ierr = VecSetFromOptions(u); CHKERRQ(ierr);
    ierr = TSSetFromOptions(ts); CHKERRQ(ierr);

    // set initial values and solve
    ierr = SetInitial(u, &user); CHKERRQ(ierr);
    ierr = TSSolve(ts, u); CHKERRQ(ierr);
    ierr = TSGetTime(ts, &tf); CHKERRQ(ierr);

    // numerical error in free case
    if (user.free) {
        ierr = VecDuplicate(u, &uexact); CHKERRQ(ierr);
        // get initial condition for evaluating exact solution
        ierr = VecDuplicate(u, &u0); CHKERRQ(ierr);
        ierr = SetInitial(u0, &user); CHKERRQ(ierr);
        ierr = FreeExact(u0, tf, uexact, &user); CHKERRQ(ierr);
        //ierr = VecView(uexact, PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);
        ierr = VecAXPY(u, -1.0, uexact); CHKERRQ(ierr); // u <- u - uexact
        ierr = VecNorm(u, NORM_INFINITY, &errnorm); CHKERRQ(ierr);
        VecDestroy(&u0);  VecDestroy(&uexact);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
               "numerical error at tf in free problem: |u-uexact|_inf = %.5e\n",
               errnorm); CHKERRQ(ierr);
    }

    // report
    ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
    if (user.free)
        strcpy(probstr, "free");
    ierr = PetscPrintf(PETSC_COMM_WORLD,
               "%s problem solved to tf = %.3f (%d steps)\n",
               probstr, tf, steps); CHKERRQ(ierr);

    MatDestroy(&A);  VecDestroy(&u);  TSDestroy(&ts);
    return PetscFinalize();
}

/* Regarding the following functions:
In the rod case the solution is a 10-dimensional vector u.
Here is the correspondence with the notes (doc/rod.pdf):
u[0] = q_1 (= x_1)
u[1] = q_2 (= y_1)
u[2] = q_3 (= x_2)
u[3] = q_4 (= y_2)
u[4] = v_1 (= dx_1/dt)
u[5] = v_2 (= dy_1/dt)
u[6] = v_3 (= dx_2/dt)
u[7] = v_4 (= dy_2/dt)
u[8] = mu
u[9] = lambda
*/

PetscErrorCode SetInitial(Vec u, RodCtx *user) {
    /* Set initial conditions compatible with the rod constraint
        0 = g(q) = (1/2) ( (q1 - q3)^2 + (q2 - q4)^2 - l^2 )    (1)
    and the velocity constraint
        0 = G(q) v = (q1 - q3)(v1 - v3) + (q2 - q4)(v2 - v4)    (2)
    The initial conditions are based on the location of the first mass
    being at cartesian location (q1,q2)=(0,1) and the second at
    (q3,q4)=(0,1+l).  The initial velocity of the first mass is
    (v1,v2)=(10,10) and the second is (v3,v4)=(15,10).  In fact we
    adjust q4 and v4 from the other values so as to satisfy constraints
    (1) and (2).  The initial value mu=0 is always set.  We set lambda
    from the other values according to the acceleration constraint
        lambda = (m / (2 l^2)) ((v1-v3)^2 + (v2-v4)^2)          (3)
    Initial values of mu and lambda are ignored in BDF solutions. */
    PetscErrorCode   ierr;
    const PetscReal  c = user->m / (2.0 * user->l * user->l);
    PetscReal        *au;
    ierr = VecGetArray(u,&au); CHKERRQ(ierr);
    au[0] = 0.0;              // q1 = x1
    au[1] = 1.0;              // q2 = y1
    au[2] = 0.0;              // q3 = x2
    au[3] = au[1] + user->l;  // q4 = y2;  satisfies (1)
    au[4] = 10.0;             // v1
    au[5] = 10.0;             // v2
    au[6] = 15.0;             // v3
    au[7] = au[5];            // v4;  satisfies (2)
    au[8] = 0.0;          // mu
    if (user->free)
        au[9] = 0.0;
    else // set lambda to satisfy (3)
        au[9] = c * ( (au[4] - au[6]) * (au[4] - au[6])
                     + (au[5] - au[7]) * (au[5] - au[7]));
    ierr = VecRestoreArray(u,&au); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FreeExact(Vec u0, PetscReal tf, Vec uexact, RodCtx *user) {
    /* Exact solution based on parabolic motion.  Problem
        m x'' = 0,       x(0) = x0,  x'(0) = v0
        m y'' = - m g,   y(0) = y0,  y'(0) = w0
    has solution
        x(t) = x0 + v0 t,              x'(t) = v0,
        y(t) = y0 + w0 t - (g/2) t^2,  y'(t) = w0 - g t    */
    PetscErrorCode ierr;
    PetscReal *au0, *auex;
    if (!user->free) {
        SETERRQ(PETSC_COMM_SELF,7,"exact solution only implemented for free\n");
    }
    ierr = VecGetArray(u0, &au0); CHKERRQ(ierr);
    ierr = VecGetArray(uexact, &auex); CHKERRQ(ierr);
    auex[0] = au0[0] + au0[4] * tf;
    auex[1] = au0[1] + au0[5] * tf - 0.5 * user->g * tf * tf;
    auex[2] = au0[2] + au0[6] * tf;
    auex[3] = au0[3] + au0[7] * tf - 0.5 * user->g * tf * tf;
    auex[4] = au0[4];
    auex[5] = au0[5] - user->g * tf;
    auex[6] = au0[6];
    auex[7] = au0[7] - user->g * tf;
    auex[8] = 0.0;  // mu
    auex[9] = 0.0;  // lambda
    ierr = VecRestoreArray(u0, &au0); CHKERRQ(ierr);
    ierr = VecRestoreArray(uexact, &auex); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode LagrangeIFcn(TS ts, PetscReal t, Vec u, Vec udot, Vec F,
                            void *ctx) {
    PetscErrorCode   ierr;
    RodCtx           *user = (RodCtx*)ctx;
    const PetscReal  *au, *audot, mg = user->m * user->g;
    PetscReal        *aF, dx, dy, dvx, dvy;

    PetscFunctionBeginUser;
    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecGetArrayRead(udot,&audot); CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF); CHKERRQ(ierr);
    dx = au[0] - au[2];
    dy = au[1] - au[3];
    dvx = au[4] - au[6];
    dvy = au[5] - au[7];
    aF[0] = audot[0] - au[4] + au[8] * dx;
    aF[1] = audot[1] - au[5] + au[8] * dy;
    aF[2] = audot[2] - au[6] - au[8] * dx;
    aF[3] = audot[3] - au[7] - au[8] * dy;
    aF[4] = user->m * audot[4] + au[9] * dx;
    aF[5] = user->m * audot[5] + mg + au[9] * dy;
    aF[6] = user->m * audot[6] - au[9] * dx;
    aF[7] = user->m * audot[7] + mg - au[9] * dy;
    if (user->free) { // trivial index 1 DAE
        aF[8] = au[8];             // equation:  mu = 0
        aF[9] = au[9];             // equation:  lambda = 0
    } else {
        aF[8] = 0.5 * ( dx * dx + dy * dy - user->l * user->l );  // constraint
        aF[9] = dx * dvx + dy * dvy;  // velocity constraint
    }
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(udot,&audot); CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF); CHKERRQ(ierr);
    PetscFunctionReturn(0);
}

PetscErrorCode LagrangeIJac(TS ts, PetscReal t, Vec u, Vec udot,
                            PetscReal sigma, Mat J, Mat Jpre, void *ctx) {
    PetscErrorCode   ierr;
    RodCtx           *user = (RodCtx*)ctx;
    const PetscReal  *au;
    PetscInt         row, col[8], n;    // max nonzeros in a row of J is 4
    PetscReal        val[8];

    PetscFunctionBeginUser;
    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    // construct Jacobian by rows, inserting nonzeros
    row = 0;  n = 4;   // row 0 has 4 nonzeros ...
    col[0] = 0;  col[1] = 2;  col[2] = 4;  col[3] = 8;
    val[0] = sigma + au[8];  val[1] = - au[8];
    val[2] = -1;             val[3] = au[0] - au[2];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 1;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 5;  col[3] = 8;
    val[0] = sigma + au[8];  val[1] = - au[8];
    val[2] = -1;             val[3] = au[1] - au[3];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 2;  n = 4;
    col[0] = 0;  col[1] = 2;  col[2] = 6;  col[3] = 8;
    val[0] = - au[8];        val[1] = sigma + au[8];
    val[2] = -1;             val[3] = -(au[0] - au[2]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 3;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 7;  col[3] = 8;
    val[0] = - au[8];        val[1] = sigma + au[8];
    val[2] = -1;             val[3] = -(au[1] - au[3]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 4;  n = 4;
    col[0] = 0;  col[1] = 2;  col[2] = 4;  col[3] = 9;
    val[0] = au[9];          val[1] = - au[9];
    val[2] = sigma*user->m;  val[3] = au[0] - au[2];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 5;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 5;  col[3] = 9;
    val[0] = au[9];          val[1] = - au[9];
    val[2] = sigma*user->m;  val[3] = au[1] - au[3];
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 6;  n = 4;
    col[0] = 0;  col[1] = 2;  col[2] = 6;  col[3] = 9;
    val[0] = -au[9];         val[1] = au[9];
    val[2] = sigma*user->m;  val[3] = -(au[0] - au[2]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    row = 7;  n = 4;
    col[0] = 1;  col[1] = 3;  col[2] = 7;  col[3] = 9;
    val[0] = -au[9];         val[1] = au[9];
    val[2] = sigma*user->m;  val[3] = -(au[1] - au[3]);
    ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    if (user->free) {
        n = 1;
        row = 8;  col[0] = 8;  val[0] = 1.0; // equation mu = 0
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
        row = 9;  col[0] = 9;  val[0] = 1.0; // equation lambda = 0
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    } else {
        n = 4;  row = 8;  n = 4;
        col[0] = 0;  col[1] = 1;  col[2] = 2;  col[3] = 3;
        val[0] = au[0] - au[2];     val[1] = au[1] - au[3];
        val[2] = -(au[0] - au[2]);  val[3] = -(au[1] - au[3]);
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
        n = 4;  row = 9;  n = 8;
        col[0] = 0;  col[1] = 1;  col[2] = 2;  col[3] = 3;
        col[4] = 4;  col[5] = 5;  col[6] = 6;  col[7] = 7;
        val[0] = au[4] - au[6];     val[1] = au[5] - au[7];
        val[2] = -(au[4] - au[6]);  val[3] = -(au[5] - au[7]);
        val[4] = au[0] - au[2];     val[5] = au[1] - au[3];
        val[6] = -(au[0] - au[2]);  val[7] = -(au[1] - au[3]);
        ierr = MatSetValues(J,1,&row,n,col,val,INSERT_VALUES); CHKERRQ(ierr);
    }
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    if (J != Jpre) {
        ierr = MatAssemblyBegin(Jpre,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
        ierr = MatAssemblyEnd(Jpre,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    }
    PetscFunctionReturn(0);
}

ext: c

max_stars_repo_name: bueler/dae-examples

max_issues_repo_name: bueler/dae-examples

max_forks_repo_name: bueler/dae-examples


text:
#pragma once

#include "Cesium3DTiles/BoundingVolume.h"
#include "Cesium3DTiles/Gltf.h"
#include "Cesium3DTiles/Library.h"
#include "Cesium3DTiles/RasterMappedTo3DTile.h"
#include "Cesium3DTiles/RasterOverlayTile.h"
#include "Cesium3DTiles/TileContext.h"
#include "Cesium3DTiles/TileID.h"
#include "Cesium3DTiles/TileRefine.h"
#include "Cesium3DTiles/TileSelectionState.h"
#include "CesiumAsync/IAssetRequest.h"
#include "CesiumGeospatial/Projection.h"
#include "CesiumUtility/DoublyLinkedList.h"
#include <atomic>
#include <glm/mat4x4.hpp>
#include <gsl/span>
#include <memory>
#include <optional>
#include <string>
#include <vector>

namespace Cesium3DTiles {
class Tileset;
class TileContent;
struct TileContentLoadResult;

/**
 * @brief A tile in a {@link Tileset}.
 *
 * The tiles of a tileset form a hierarchy, where each tile may contain
 * renderable content, and each tile has an associated bounding volume.
 *
 * The actual hierarchy is represented with the {@link Tile::getParent}
 * and {@link Tile::getChildren} functions.
 *
 * The renderable content is provided as a {@link TileContentLoadResult}
 * from the {@link Tile::getContent} function.
 * The {@link Tile::getGeometricError} function returns the geometric
 * error of the representation of the renderable content of a tile.
 *
 * The {@link BoundingVolume} is given by the {@link Tile::getBoundingVolume}
 * function. This bounding volume encloses the renderable content of the
 * tile itself, as well as the renderable content of all children, yielding
 * a spatially coherent hierarchy of bounding volumes.
 *
 * The bounding volume of the content of an individual tile is given
 * by the {@link Tile::getContentBoundingVolume} function.
 *
 */
class CESIUM3DTILES_API Tile final {
public:
  /**
   * The current state of this tile in the loading process.
   */
  enum class LoadState {
    /**
     * @brief This tile is in the process of being destroyed.
     *
     * Any pointers to it will soon be invalid.
     */
    Destroying = -3,

    /**
     * @brief Something went wrong while loading this tile and it will not be
     * retried.
     */
    Failed = -2,

    /**
     * @brief Something went wrong while loading this tile, but it may be a
     * temporary problem.
     */
    FailedTemporarily = -1,

    /**
     * @brief The tile is not yet loaded at all, beyond the metadata in
     * tileset.json.
     */
    Unloaded = 0,

    /**
     * @brief The tile content is currently being loaded.
     *
     * Note that while a tile is in this state, its {@link Tile::getContent},
     * and {@link Tile::getState}, methods may be called from the load thread,
     * and the state may change due to the internal loading process.
     */
    ContentLoading = 1,

    /**
     * @brief The tile content has finished loading.
     */
    ContentLoaded = 2,

    /**
     * @brief The tile is completely done loading.
     */
    Done = 3
  };

  /**
   * @brief Default constructor for an empty, uninitialized tile.
   */
  Tile() noexcept;

  /**
   * @brief Default destructor, which clears all resources associated with this
   * tile.
   */
  ~Tile();

  /**
   * @brief Copy constructor.
   *
   * @param rhs The other instance.
   */
  Tile(Tile& rhs) noexcept = delete;

  /**
   * @brief Move constructor.
   *
   * @param rhs The other instance.
   */
  Tile(Tile&& rhs) noexcept;

  /**
   * @brief Move assignment operator.
   *
   * @param rhs The other instance.
   */
  Tile& operator=(Tile&& rhs) noexcept;

  /**
   * @brief Returns the {@link Tileset} to which this tile belongs.
   */
  Tileset* getTileset() noexcept { return this->_pContext->pTileset; }

  /** @copydoc Tile::getTileset() */
  const Tileset* getTileset() const noexcept {
    return this->_pContext->pTileset;
  }

  /**
   * @brief Returns the {@link TileContext} of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @return The tile context.
   */
  TileContext* getContext() noexcept { return this->_pContext; }

  /** @copydoc Tile::getContext() */
  const TileContext* getContext() const noexcept { return this->_pContext; }

  /**
   * @brief Set the {@link TileContext} of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param pContext The tile context.
   */
  void setContext(TileContext* pContext) noexcept {
    this->_pContext = pContext;
  }

  /**
   * @brief Returns the parent of this tile in the tile hierarchy.
   *
   * This will be the `nullptr` if this is the root tile.
   *
   * @return The parent.
   */
  Tile* getParent() noexcept { return this->_pParent; }

  /** @copydoc Tile::getParent() */
  const Tile* getParent() const noexcept { return this->_pParent; }

  /**
   * @brief Set the parent of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param pParent The parent tile .
   */
  void setParent(Tile* pParent) noexcept { this->_pParent = pParent; }

  /**
   * @brief Returns a *view* on the children of this tile.
   *
   * The returned span will become invalid when this tile is destroyed.
   *
   * @return The children of this tile.
   */
  gsl::span<Tile> getChildren() noexcept {
    return gsl::span<Tile>(this->_children);
  }

  /** @copydoc Tile::getChildren() */
  gsl::span<const Tile> getChildren() const noexcept {
    return gsl::span<const Tile>(this->_children);
  }

  /**
   * @brief Allocates space for the given number of child tiles.
   *
   * This function is not supposed to be called by clients.
   *
   * @param count The number of child tiles.
   * @throws `std::runtime_error` if this tile already has children.
   */
  void createChildTiles(size_t count);

  /**
   * @brief Assigns the given child tiles to this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param children The child tiles.
   * @throws `std::runtime_error` if this tile already has children.
   */
  void createChildTiles(std::vector<Tile>&& children);

  /**
   * @brief Returns the {@link BoundingVolume} of this tile.
   *
   * This is a bounding volume that encloses the content of this tile,
   * as well as the content of all child tiles.
   *
   * @see Tile::getContentBoundingVolume
   *
   * @return The bounding volume.
   */
  const BoundingVolume& getBoundingVolume() const noexcept {
    return this->_boundingVolume;
  }

  /**
   * @brief Set the {@link BoundingVolume} of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param value The bounding volume.
   */
  void setBoundingVolume(const BoundingVolume& value) noexcept {
    this->_boundingVolume = value;
  }

  /**
   * @brief Returns the viewer request volume of this tile.
   *
   * The viewer request volume is an optional {@link BoundingVolume} that
   * may be associated with a tile. It allows controlling the rendering
   * process of the tile content: If the viewer request volume is present,
   * then the content of the tile will only be rendered when the viewer
   * (i.e. the camera position) is inside the viewer request volume.
   *
   * @return The viewer request volume, or an empty optional.
   */
  const std::optional<BoundingVolume>& getViewerRequestVolume() const noexcept {
    return this->_viewerRequestVolume;
  }

  /**
   * @brief Set the viewer request volume of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param value The viewer request volume.
   */
  void
  setViewerRequestVolume(const std::optional<BoundingVolume>& value) noexcept {
    this->_viewerRequestVolume = value;
  }

  /**
   * @brief Returns the geometric error of this tile.
   *
   * This is the error, in meters, introduced if this tile is rendered and its
   * children are not. This is used to compute screen space error, i.e., the
   * error measured in pixels.
   *
   * @return The geometric error of this tile, in meters.
   */
  double getGeometricError() const noexcept { return this->_geometricError; }

  /**
   * @brief Set the geometric error of the contents of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param value The geometric error, in meters.
   */
  void setGeometricError(double value) noexcept {
    this->_geometricError = value;
  }

  /**
   * @brief The refinement strategy of this tile.
   *
   * Returns the {@link TileRefine} value that indicates the refinement strategy
   * for this tile. This is `Add` when the content of the
   * child tiles is *added* to the content of this tile during refinement, and
   * `Replace` when the content of the child tiles *replaces*
   * the content of this tile during refinement.
   *
   * @return The refinement strategy.
   */
  TileRefine getRefine() const noexcept { return this->_refine; }

  /**
   * @brief Set the refinement strategy of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param value The refinement strategy.
   */
  void setRefine(TileRefine value) noexcept { this->_refine = value; }

  /**
   * @brief Gets the transformation matrix for this tile.
   *
   * This matrix does _not_ need to be multiplied with the tile's parent's
   * transform as this has already been done.
   *
   * @return The transform matrix.
   */
  const glm::dmat4x4& getTransform() const noexcept { return this->_transform; }

  /**
   * @brief Set the transformation matrix for this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param value The transform matrix.
   */
  void setTransform(const glm::dmat4x4& value) noexcept {
    this->_transform = value;
  }

  /**
   * @brief Returns the {@link TileID} of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @return The tile ID.
   */
  const TileID& getTileID() const noexcept { return this->_id; }

  /**
   * @brief Set the {@link TileID} of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param id The tile ID.
   */
  void setTileID(const TileID& id) noexcept;

  /**
   * @brief Returns the {@link BoundingVolume} of the renderable content of this
   * tile.
   *
   * The content bounding volume is a bounding volume that tightly fits only the
   * renderable content of the tile. This enables tighter view frustum culling,
   * making it possible to exclude from rendering any content not in the view
   * frustum.
   *
   * @see Tile::getBoundingVolume
   */
  const std::optional<BoundingVolume>&
  getContentBoundingVolume() const noexcept {
    return this->_contentBoundingVolume;
  }

  /**
   * @brief Set the {@link BoundingVolume} of the renderable content of this
   * tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param value The content bounding volume
   */
  void setContentBoundingVolume(
      const std::optional<BoundingVolume>& value) noexcept {
    this->_contentBoundingVolume = value;
  }

  /**
   * @brief Returns the {@link TileContentLoadResult} for the content of this
   * tile.
   *
   * This will be a `nullptr` if the content of this tile has not yet been
   * loaded, as indicated by the indicated by the {@link Tile::getState} of this
   * tile not being {@link Tile::LoadState::ContentLoaded}.
   *
   * @return The tile content load result, or `nullptr` if no content is loaded
   */
  TileContentLoadResult* getContent() noexcept { return this->_pContent.get(); }

  /** @copydoc Tile::getContent() */
  const TileContentLoadResult* getContent() const noexcept {
    return this->_pContent.get();
  }

  /**
   * @brief Returns internal resources required for rendering this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @return The renderer resources.
   */
  void* getRendererResources() const noexcept {
    return this->_pRendererResources;
  }

  /**
   * @brief Returns the {@link LoadState} of this tile.
   */
  LoadState getState() const noexcept {
    return this->_state.load(std::memory_order::memory_order_acquire);
  }

  /**
   * @brief Returns the {@link TileSelectionState} of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @return The last selection state
   */
  TileSelectionState& getLastSelectionState() noexcept {
    return this->_lastSelectionState;
  }

  /** @copydoc Tile::getLastSelectionState() */
  const TileSelectionState& getLastSelectionState() const noexcept {
    return this->_lastSelectionState;
  }

  /**
   * @brief Set the {@link TileSelectionState} of this tile.
   *
   * This function is not supposed to be called by clients.
   *
   * @param newState The new stace
   */
  void setLastSelectionState(const TileSelectionState& newState) noexcept {
    this->_lastSelectionState = newState;
  }

  /**
   * @brief Returns the raster overlay tiles that have been mapped to this tile.
   */
  std::vector<RasterMappedTo3DTile>& getMappedRasterTiles() noexcept {
    return this->_rasterTiles;
  }

  /** @copydoc Tile::getMappedRasterTiles() */
  const std::vector<RasterMappedTo3DTile>&
  getMappedRasterTiles() const noexcept {
    return this->_rasterTiles;
  }

  /**
   * @brief Determines if this tile is currently renderable.
   */
  bool isRenderable() const noexcept;

  /**
   * @brief Trigger the process of loading the {@link Tile::getContent}.
   *
   * This function is not supposed to be called by clients.
   *
   * If this tile is not in its initial state (indicated by the
   * {@link Tile::getState} of this tile being *not*
   * {@link Tile::LoadState::Unloaded}), then nothing will be done.
   *
   * Otherwise, the tile will go into the
   * {@link Tile::LoadState::ContentLoading} state, and the request for
   * loading the tile content will be sent out.
   * The function will then return, and the response of the request will
   * be received asynchronously. Depending on the type of the tile and
   * the response, the tile will eventually go into the
   * {@link Tile::LoadState::ContentLoaded} state, and the
   * {@link Tile::getContent} will be available.
   */
  void loadContent();

  /**
   * @brief Frees all resources that have been allocated for the
   * {@link Tile::getContent}.
   *
   * This function is not supposed to be called by clients.
   *
   * If the operation for loading the tile content is currently in progress, as
   * indicated by the {@link Tile::getState} of this tile being
   * {@link Tile::LoadState::ContentLoading}), then nothing will be done,
   * and `false` will be returned.
   *
   * Otherwise, the resources that have been allocated for the tile content will
   * be freed.
   *
   * @return Whether the content was unloaded.
   */
  bool unloadContent() noexcept;

  /**
   * @brief Gives this tile a chance to update itself each render frame.
   *
   * @param previousFrameNumber The number of the previous render frame.
   * @param currentFrameNumber The number of the current render frame.
   */
  void update(int32_t previousFrameNumber, int32_t currentFrameNumber);

  /**
   * @brief Marks the tile as permanently failing to load.
   *
   * This function is not supposed to be called by clients.
   *
   * Moves the tile from the `FailedTemporarily` state to the `Failed` state.
   * If the tile is not in the `FailedTemporarily` state, this method does
   * nothing.
   */
  void markPermanentlyFailed() noexcept;

  /**
   * @brief Determines the number of bytes in this tile's geometry and texture
   * data.
   */
  int64_t computeByteSize() const noexcept;

private:
  /**
   * @brief Set the {@link LoadState} of this tile.
   */
  void setState(LoadState value) noexcept;

  /**
   * @brief Generates texture coordiantes for the raster overlays of the content
   * of this tile.
   *
   * This will extend the accessors of the glTF model of the content of this
   * tile with accessors that contain the texture coordinate sets for different
   * projections. Further details are not specified here.
   *
   * @return The bounding region
   */
  static std::optional<CesiumGeospatial::BoundingRegion>
  generateTextureCoordinates(
      CesiumGltf::Model& model,
      const BoundingVolume& boundingVolume,
      const std::vector<CesiumGeospatial::Projection>& projections);

  /**
   * @brief Upsample the parent of this tile.
   *
   * This method should only be called when this tile's parent is already
   * loaded.
   */
  void upsampleParent(std::vector<CesiumGeospatial::Projection>&& projections);

  // Position in bounding-volume hierarchy.
  TileContext* _pContext;
  Tile* _pParent;
  std::vector<Tile> _children;

  // Properties from tileset.json.
  // These are immutable after the tile leaves TileState::Unloaded.
  BoundingVolume _boundingVolume;
  std::optional<BoundingVolume> _viewerRequestVolume;
  double _geometricError;
  TileRefine _refine;
  glm::dmat4x4 _transform;

  TileID _id;
  std::optional<BoundingVolume> _contentBoundingVolume;

  // Load state and data.
  std::atomic<LoadState> _state;
  std::unique_ptr<TileContentLoadResult> _pContent;
  void* _pRendererResources;

  // Selection state
  TileSelectionState _lastSelectionState;

  // Overlays
  std::vector<RasterMappedTo3DTile> _rasterTiles;

  CesiumUtility::DoublyLinkedListPointers<Tile> _loadedTilesLinks;

public:
  /**
   * @brief A {@link CesiumUtility::DoublyLinkedList} for tile objects.
   */
  typedef CesiumUtility::DoublyLinkedList<Tile, &Tile::_loadedTilesLinks>
      LoadedLinkedList;
};

} // namespace Cesium3DTiles