#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <gsl/gsl_rng.h>

#ifdef SUBFIND

#include "fof.h"

#include "allvars.h"
#include "proto.h"
#include "domain.h"
#include "subfind.h"

static struct id_list
{
  MyIDType ID;
  int GrNr;
  int SubNr;
  float BindingEgy;

#ifdef SUBFIND_SAVE_PARTICLELISTS
  float Pos[3];
  float Vel[3];
  int Type;
#ifdef STELLARAGE
  float Mass;
  float StellarAge;
#endif
#endif
}
 *ID_list;

static int Nids;

void subfind(int num)
{
  double t0, t1, tstart, tend;
  int i, gr, nlocid, offset, limit, ncount;

#ifdef DENSITY_SPLIT_BY_TYPE
  struct unbind_data *d;
  int j, n, count[6], countall[6];
  double a3inv, dmax1, dmax2;
#endif

  if(ThisTask == 0)
    printf("\nWe now execute a parallel version of SUBFIND.\n");

  tstart = second();

#ifndef ONLY_PRODUCE_HSML_FILES
  if(!All.ComovingIntegrationOn)
    {
      if(ThisTask == 0)
	printf("works only for comoving integration.\n");
      endrun(0);
    }
#endif

#ifdef DENSITY_SPLIT_BY_TYPE
  a3inv = 1 / (All.Time * All.Time * All.Time);

  for(j = 0; j < 6; j++)
    count[j] = 0;

  /* let's count number of particles of selected species */
  for(i = 0; i < NumPart; i++)
    count[P[i].Type]++;

  MPI_Allreduce(count, countall, 6, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

  /* do first loop: basically just defining the hsml for different species */
  for(j = 0; j < 6; j++)
    {
      if((1 << j) & (DENSITY_SPLIT_BY_TYPE))
	{
#ifdef BLACK_HOLES
	  if(j == 5)
	    countall[j] = 0;	/* this will prevent that the black holes are treated separately */
#endif

	  force_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

	  if(countall[j] > All.DesNumNgb)
	    {
	      /* build index list of particles of selectes species */
	      d = (struct unbind_data *) mymalloc(count[j] * sizeof(struct unbind_data));
	      for(i = 0, n = 0; i < NumPart; i++)
		if(P[i].Type == j)
		  d[n++].index = i;

	      t0 = second();
	      if(ThisTask == 0)
		printf("Tree construction for species %d (%d).\n", j, countall[j]);

	      CPU_Step[CPU_FOF] += measure_time();

	      force_treebuild(count[j], d);

	      myfree(d);

	      t1 = second();
	      if(ThisTask == 0)
		printf("tree build for species %d took %g sec\n", j, timediff(t0, t1));
	    }
	  else
	    {
	      t0 = second();
	      if(ThisTask == 0)
		printf("Tree construction.\n");

	      CPU_Step[CPU_FOF] += measure_time();

	      force_treebuild(NumPart, NULL);

	      t1 = second();
	      if(ThisTask == 0)
		printf("tree build took %g sec\n", timediff(t0, t1));
	    }

	  /* let's determine the local densities */
	  t0 = second();
	  subfind_setup_smoothinglengths(j);
	  subfind_density(j);
	  t1 = second();
	  if(ThisTask == 0)
	    printf("density and smoothing length for species %d took %g sec\n", j, timediff(t0, t1));

	  force_treefree();

	  /* let's save density contribution of own species */
	  for(i = 0; i < NumPart; i++)
	    if(P[i].Type == j)
	      P[i].w.density_sum = P[i].u.DM_Density;

	}
    }

  /* do second loop: now calculate all density contributions */
  for(j = 0; j < 6; j++)
    {
      if((1 << j) & (DENSITY_SPLIT_BY_TYPE))
	{
	  force_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

	  /* build index list of particles of selectes species */
	  d = (struct unbind_data *) mymalloc(count[j] * sizeof(struct unbind_data));
	  for(i = 0, n = 0; i < NumPart; i++)
	    if(P[i].Type == j)
	      d[n++].index = i;

	  t0 = second();
	  if(ThisTask == 0)
	    printf("Tree construction for species %d (%d).\n", j, countall[j]);

	  CPU_Step[CPU_FOF] += measure_time();

	  force_treebuild(count[j], d);

	  myfree(d);

	  t1 = second();
	  if(ThisTask == 0)
	    printf("tree build for species %d took %g sec\n", j, timediff(t0, t1));

	  /* let's determine the local densities */
	  t0 = second();
	  for(i = 0; i < 6; i++)
	    if((1 << i) & (DENSITY_SPLIT_BY_TYPE))
	      if(j != i)
		{
		  if(countall[i] > All.DesNumNgb)
		    {
		      if(ThisTask == 0)
			printf("calculating density contribution of species %d to species %d\n", j, i);
		      subfind_density(-(i + 1));
		    }
		}
	  t1 = second();
	  if(ThisTask == 0)
	    printf("density() of species %d took %g sec\n", j, timediff(t0, t1));

	  force_treefree();

	  /* let's sum up density contribution */
	  for(i = 0; i < NumPart; i++)
	    if((1 << P[i].Type) & (DENSITY_SPLIT_BY_TYPE))
	      if(j != P[i].Type)
		if(countall[P[i].Type] > All.DesNumNgb)
		  P[i].w.density_sum += P[i].u.DM_Density;
	}
    }

  for(i = 0; i < NumPart; i++)
    {
      P[i].u.DM_Density = P[i].w.density_sum;

      if(P[i].Type == 0)
	P[i].w.int_energy = DMAX(All.MinEgySpec,
				 SphP[i].Entropy / GAMMA_MINUS1 * pow(SphP[i].d.Density * a3inv,
								      GAMMA_MINUS1));
      else
	P[i].w.int_energy = 0;

    }
#else
  force_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

  t0 = second();
  if(ThisTask == 0)
    printf("Tree construction.\n");

  CPU_Step[CPU_FOF] += measure_time();

  force_treebuild(NumPart, NULL);

  t1 = second();
  if(ThisTask == 0)
    printf("tree build took %g sec\n", timediff(t0, t1));

  /* let's determine the local dark matter densities */
  t0 = second();
  subfind_setup_smoothinglengths();
  subfind_density();
  t1 = second();
  if(ThisTask == 0)
    printf("dark matter density() took %g sec\n", timediff(t0, t1));

  force_treefree();
#endif /* DENSITY_SPLIT_BY_TYPE */

  if(DumpFlag)
    {
      /* let's save the densities to a file (for making images) */
      t0 = second();
      subfind_save_densities(num);
      t1 = second();
      if(ThisTask == 0)
	printf("saving densities took %g sec\n", timediff(t0, t1));
    }

#ifdef ONLY_PRODUCE_HSML_FILES
  return;
#endif

  /* count how many groups we have that should be done collectively */
  limit = 0.6 * All.TotNumPart / NTask;

  for(i = 0, ncount = 0; i < Ngroups; i++)
    if(Group[i].Len >= limit)
      ncount++;
  MPI_Allreduce(&ncount, &Ncollective, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

  if(ThisTask == 0)
    {
      printf("\nNumber of FOF halos treated with collective SubFind code = %d\n", Ncollective);
      printf("(the adopted size-limit for the collective algorithm was %d particles.)\n", limit);
      printf("the other %d FOF halos are treated in parallel with serial code\n\n", TotNgroups - Ncollective);
    }

  /*  to decide on which task a group should be:
   *  if   GrNr <= Ncollective:  collective groupfinding.
   *  the task where the group info is put is TaskNr = (GrNr - 1) % NTask
   */

  /* now we distribute the particles such that small groups are assigned in
   *  total to certain CPUs, and big groups are left where they are
   */

  t0 = second();

  for(i = 0; i < NumPart; i++)
    {
      if(P[i].GrNr > Ncollective && P[i].GrNr <= TotNgroups)	/* particle is in small group */
	P[i].targettask = (P[i].GrNr - 1) % NTask;
      else
	P[i].targettask = ThisTask;
    }

  subfind_exchange();		/* distributes gas particles as well if needed */

  t1 = second();
  if(ThisTask == 0)
    printf("subfind_exchange()() took %g sec\n", timediff(t0, t1));

  subfind_distribute_groups();

  qsort(Group, Ngroups, sizeof(struct group_properties), fof_compare_Group_GrNr);

  for(i = 0; i < NumPart; i++)
    if(P[i].GrNr > Ncollective && P[i].GrNr <= TotNgroups)
      if(((P[i].GrNr - 1) % NTask) != ThisTask)
	{
	  printf("i=%d %d task=%d\n", i, P[i].GrNr, ThisTask);
	  endrun(87);
	}

  /* lets estimate the maximum number of substructures we need to store on the local CPU */
  for(i = 0, nlocid = 0; i < Ngroups; i++)
    nlocid += Group[i].Len;

  MaxNsubgroups = nlocid / All.DesLinkNgb;	/* this is a quite conservative upper limit */
  Nsubgroups = 0;
  SubGroup = (struct subgroup_properties *) mymalloc(MaxNsubgroups * sizeof(struct subgroup_properties));

  for(i = 0; i < NumPart; i++)
    P[i].SubNr = (1 << 30);	/* default */

  /* we begin by applying the collective version of subfind to distributed groups */
  t0 = second();
  for(GrNr = 1; GrNr <= Ncollective; GrNr++)
    subfind_process_group_collectively(num);
  t1 = second();
  if(ThisTask == 0)
    printf("processing of collective halos took %g sec\n", timediff(t0, t1));

#ifdef SUBFIND_COLLECTIVE_STAGE1
  if(ThisTask == 0)
    printf("stage 1 ended\n");
  endrun(0);
#endif

  for(i = 0; i < NumPart; i++)
    {
      P[i].origindex = i;
      P[i].origintask = ThisTask;
    }

  t0 = second();
  qsort(P, NumPart, sizeof(struct particle_data), subfind_compare_P_GrNr_DM_Density);
  t1 = second();
  if(ThisTask == 0)
    printf("sort of local particles()() took %g sec\n", timediff(t0, t1));

  /* now we have the particles of groups consecutively, but SPH particles are
     not aligned. They can however be accessed via SphP[P[i].originindex] */

  /* let's count how many local particles we have in small groups */
  for(i = 0, nlocid = 0; i < NumPart; i++)
    if(P[i].GrNr > Ncollective && P[i].GrNr <= Ngroups)	/* particle is in small group */
      nlocid++;

  if(ThisTask == 0)
    printf("contructing tree for serial subfind of local groups\n");

  subfind_loctree_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

  if(ThisTask == 0)
    printf("Start to do local groups with serial subfind algorithm\n");

  t0 = second();

  /* we now apply a serial version of subfind to the local groups */
  for(gr = 0, offset = 0; gr < Ngroups; gr++)
    {
      if(Group[gr].GrNr > Ncollective)
	{
	  if(((Group[gr].GrNr - 1) % NTask) == ThisTask)
	    offset = subfind_process_group_serial(gr, offset);
	}
    }

  t1 = second();
  if(ThisTask == 0)
    printf("processing of local groups took took %g sec\n", timediff(t0, t1));

  subfind_loctree_treefree();

  /* bringing back particles in original positions, such that gas particles are aligned */
  t0 = second();
  qsort(P, NumPart, sizeof(struct particle_data), subfind_compare_P_origindex);
  t1 = second();
  if(ThisTask == 0)
    printf("unsorting of local particles()() took %g sec\n", timediff(t0, t1));

  GrNr = -1;			/* to ensure that domain decomposition acts normally again */

  /* now determine the remaining spherical overdensity values for the non-local groups */

  domain_free_trick();

  CPU_Step[CPU_FOF] += measure_time();

#ifdef DENSITY_SPLIT_BY_TYPE
  printf("Task %d: testing particles ...\n", ThisTask);
  for(i = 0; i < NumPart; i++)
    {
      if(P[i].origintask != ThisTask)
	printf("Task %d: Holding particle of task %d !\n", ThisTask, P[i].origintask);
      if(P[i].origindex != i)
	printf("Task %d: Particles is in wrong position (is=%d, was=%d) !\n", ThisTask, i, P[i].origindex);
    }
#endif

  All.DoDynamicUpdate = 0;
  domain_Decomposition();

  force_treebuild(NumPart, NULL);

  /* compute spherical overdensities for FOF groups */
  t0 = second();

  subfind_overdensity();

  t1 = second();
  if(ThisTask == 0)
    printf("determining spherical overdensity masses took %g sec\n", timediff(t0, t1));

  /* determine which halos are contaminated by boundary particles */
  t0 = second();

  subfind_contamination();

  t1 = second();
  if(ThisTask == 0)
    printf("determining contamination of halos took %g sec\n", timediff(t0, t1));

  force_treefree();
  domain_free();

  domain_allocate_trick();

  /* now assemble final output */
  subfind_save_final(num);

  tend = second();

  if(ThisTask == 0)
    printf("\nFinished with SUBFIND.  (total time=%g sec)\n\n", timediff(tstart, tend));

  myfree(SubGroup);

  CPU_Step[CPU_FOF] += measure_time();
}

void subfind_save_final(int num)
{
  int i, j, totsubs, masterTask, groupTask, nprocgroup;
  char buf[1000];
  double t0, t1;

  /* prepare list of ids with assigned group numbers */

  parallel_sort(Group, Ngroups, sizeof(struct group_properties), fof_compare_Group_GrNr);
  parallel_sort(SubGroup, Nsubgroups, sizeof(struct subgroup_properties),
		subfind_compare_SubGroup_GrNr_SubNr);

  ID_list = mymalloc(sizeof(struct id_list) * NumPart);

  for(i = 0, Nids = 0; i < NumPart; i++)
    {
      if(P[i].GrNr <= TotNgroups)
	{
	  ID_list[Nids].GrNr = P[i].GrNr;
	  ID_list[Nids].SubNr = P[i].SubNr;
	  ID_list[Nids].BindingEgy = P[i].v.DM_BindingEnergy;
	  ID_list[Nids].ID = P[i].ID;
#ifdef SUBFIND_SAVE_PARTICLELISTS
	  for(j = 0; j < 3; j++)
	    {
	      ID_list[Nids].Pos[j] = P[i].Pos[j];
	      ID_list[Nids].Vel[j] = P[i].Vel[j];
	    }
	  ID_list[Nids].Type = P[i].Type;
#ifdef STELLARAGE
          ID_list[Nids].Mass = P[i].Mass;
          if(P[i].Type == 4)
            ID_list[Nids].StellarAge = P[i].StellarAge;
          else
            ID_list[Nids].StellarAge = 0;
#endif
#endif
	  Nids++;
	}
    }

  parallel_sort(ID_list, Nids, sizeof(struct id_list), subfind_compare_ID_list);

  MPI_Allreduce(&Nsubgroups, &TotNsubgroups, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

  /* fill in the FirstSub-values */
  for(i = 0, totsubs = 0; i < Ngroups; i++)
    {
      if(i > 0)
	Group[i].FirstSub = Group[i - 1].FirstSub + Group[i - 1].Nsubs;
      else
	Group[i].FirstSub = 0;
      totsubs += Group[i].Nsubs;
    }

  MPI_Allgather(&totsubs, 1, MPI_INT, Send_count, 1, MPI_INT, MPI_COMM_WORLD);
  for(j = 1, Send_offset[0] = 0; j < NTask; j++)
    Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];

  for(i = 0; i < Ngroups; i++)
    Group[i].FirstSub += Send_offset[ThisTask];

  MPI_Allgather(&Nids, 1, MPI_INT, Send_count, 1, MPI_INT, MPI_COMM_WORLD);
  for(j = 1, Send_offset[0] = 0; j < NTask; j++)
    Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];

  if(ThisTask == 0)
    {
      sprintf(buf, "%s/groups_%03d", All.OutputDir, num);
      mkdir(buf, 02755);
    }
  MPI_Barrier(MPI_COMM_WORLD);

  if(NTask < All.NumFilesWrittenInParallel)
    {
      printf
	("Fatal error.\nNumber of processors must be a smaller or equal than `NumFilesWrittenInParallel'.\n");
      endrun(241931);
    }

  t0 = second();

  nprocgroup = NTask / All.NumFilesWrittenInParallel;
  if((NTask % All.NumFilesWrittenInParallel))
    nprocgroup++;
  masterTask = (ThisTask / nprocgroup) * nprocgroup;
  for(groupTask = 0; groupTask < nprocgroup; groupTask++)
    {
      if(ThisTask == (masterTask + groupTask))	/* ok, it's this processor's turn */
	subfind_save_local_catalogue(num);
      MPI_Barrier(MPI_COMM_WORLD);	/* wait inside the group */
    }

  t1 = second();

  if(ThisTask == 0)
    {
      printf("Subgroup catalogues saved. took = %g sec\n", timediff(t0, t1));
      fflush(stdout);
    }

  myfree(ID_list);
}

void subfind_save_local_catalogue(int num)
{
  FILE *fd;
  char buf[500], fname[500];
  float *mass, *pos, *vel, *spin;

#ifdef SAVE_MASS_TAB
  float *masstab;
#endif
  int i, j, *len;
  MyIDType *ids;

  sprintf(fname, "%s/groups_%03d/%s_%03d.%d", All.OutputDir, num, "subhalo_tab", num, ThisTask);
  strcpy(buf, fname);
  if(!(fd = fopen(buf, "w")))
    {
      printf("can't open file `%s`\n", buf);
      endrun(1183);
    }

  my_fwrite(&Ngroups, sizeof(int), 1, fd);
  my_fwrite(&TotNgroups, sizeof(int), 1, fd);
  my_fwrite(&Nids, sizeof(int), 1, fd);
  my_fwrite(&TotNids, sizeof(long long), 1, fd);
  my_fwrite(&NTask, sizeof(int), 1, fd);
  my_fwrite(&Nsubgroups, sizeof(int), 1, fd);
  my_fwrite(&TotNsubgroups, sizeof(int), 1, fd);

  /* group len */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].Len;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* offset into id-list */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].Offset;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* mass */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].Mass;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* location (potential minimum) */
  pos = mymalloc(Ngroups * 3 * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    for(j = 0; j < 3; j++)
      pos[i * 3 + j] = Group[i].Pos[j];
  my_fwrite(pos, Ngroups, 3 * sizeof(float), fd);
  myfree(pos);

  /* M_Mean200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].M_Mean200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* R_Mean200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].R_Mean200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* M_Crit200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].M_Crit200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* R_Crit200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].R_Crit200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* M_TopHat200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].M_TopHat200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* R_TopHat200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].R_TopHat200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

#ifdef SO_VEL_DISPERSIONS
  /* VelDisp_Mean200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].VelDisp_Mean200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* VelDisp_Crit200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].VelDisp_Crit200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* VelDisp_TopHat200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].VelDisp_TopHat200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);
#endif

  /* contamination particle count */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].ContaminationLen;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* contamination mass */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].ContaminationMass;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* number of substructures in FOF group  */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].Nsubs;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* first substructure in FOF group  */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].FirstSub;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* ------------------------------ */

  /* Len of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].Len;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* offset of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].Offset;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* parent of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].SubParent;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* Mass of substructure  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].Mass;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* Pos of substructure  */
  pos = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      pos[i * 3 + j] = SubGroup[i].Pos[j];
  my_fwrite(pos, Nsubgroups, 3 * sizeof(float), fd);
  myfree(pos);

  /* Vel of substructure  */
  vel = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      vel[i * 3 + j] = SubGroup[i].Vel[j];
  my_fwrite(vel, Nsubgroups, 3 * sizeof(float), fd);
  myfree(vel);

  /* Center of mass of substructure  */
  pos = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      pos[i * 3 + j] = SubGroup[i].CM[j];
  my_fwrite(pos, Nsubgroups, 3 * sizeof(float), fd);
  myfree(pos);

  /* Spin of substructure  */
  spin = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      spin[i * 3 + j] = SubGroup[i].Spin[j];
  my_fwrite(spin, Nsubgroups, 3 * sizeof(float), fd);
  myfree(spin);

  /* velocity dispesion  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubVelDisp;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* maximum circular velocity  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubVmax;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* radius of maximum circular velocity  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubVmaxRad;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* radius of half the mass  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubHalfMass;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* ID of most bound particle  */
  ids = mymalloc(Nsubgroups * sizeof(MyIDType));
  for(i = 0; i < Nsubgroups; i++)
    ids[i] = SubGroup[i].SubMostBoundID;
  my_fwrite(ids, Nsubgroups, sizeof(MyIDType), fd);
  myfree(ids);

  /* GrNr of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].GrNr;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* Masstab of substructure  */
#ifdef SAVE_MASS_TAB
  masstab = mymalloc(Nsubgroups * 6 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 6; j++)
      masstab[i * 6 + j] = SubGroup[i].MassTab[j];
  my_fwrite(masstab, Nsubgroups, 6 * sizeof(float), fd);
  myfree(masstab);
#endif

  fclose(fd);

#ifdef SUBFIND_SAVE_PARTICLELISTS
  sprintf(buf, "%s/groups_%03d/%s_%03d.%d", All.OutputDir, num, "subhalo_posvel", num, ThisTask);
  if(!(fd = fopen(buf, "w")))
    {
      printf("can't open file `%s`\n", buf);
      endrun(1184);
    }

  my_fwrite(&Ngroups, sizeof(int), 1, fd);
  my_fwrite(&TotNgroups, sizeof(int), 1, fd);
  my_fwrite(&Nids, sizeof(int), 1, fd);
  my_fwrite(&TotNids, sizeof(long long), 1, fd);
  my_fwrite(&NTask, sizeof(int), 1, fd);
  my_fwrite(&Send_offset[ThisTask], sizeof(int), 1, fd);
  my_fwrite(&All.Time, sizeof(double), 1, fd);

  float *posdata;
  double a3inv;
  char *types;

  if(All.ComovingIntegrationOn)
    a3inv = 1.0 / (All.Time * All.Time * All.Time);
  else
    a3inv = 1.0;

  posdata = (float *) mymalloc(3 * Nids * sizeof(float));

  for(i = 0; i < Nids; i++)
    for(j = 0; j < 3; j++)
      posdata[i * 3 + j] = ID_list[i].Pos[j];

  my_fwrite(posdata, 3 * sizeof(float), Nids, fd);

  for(i = 0; i < Nids; i++)
    for(j = 0; j < 3; j++)
      posdata[i * 3 + j] = ID_list[i].Vel[j] * sqrt(a3inv);

  my_fwrite(posdata, 3 * sizeof(float), Nids, fd);

  types = (char *) posdata;

  for(i = 0; i < Nids; i++)
    types[i] = ID_list[i].Type;

  my_fwrite(types, sizeof(char), Nids, fd);

#ifdef STELLARAGE
  for(i = 0; i < Nids; i++)
    posdata[i] = ID_list[i].Mass;

  my_fwrite(posdata, sizeof(float), Nids, fd);

  for(i = 0; i < Nids; i++)
    posdata[i] = ID_list[i].StellarAge;

  my_fwrite(posdata, sizeof(float), Nids, fd);
#endif

  myfree(posdata);

  fclose(fd);
#endif

  ids = (MyIDType *) ID_list;

  for(i = 0; i < Nids; i++)
    ids[i] = ID_list[i].ID;

  sprintf(buf, "%s/groups_%03d/%s_%03d.%d", All.OutputDir, num, "subhalo_ids", num, ThisTask);
  if(!(fd = fopen(buf, "w")))
    {
      printf("can't open file `%s`\n", buf);
      endrun(1184);
    }

  my_fwrite(&Ngroups, sizeof(int), 1, fd);
  my_fwrite(&TotNgroups, sizeof(int), 1, fd);
  my_fwrite(&Nids, sizeof(int), 1, fd);
  my_fwrite(&TotNids, sizeof(long long), 1, fd);
  my_fwrite(&NTask, sizeof(int), 1, fd);
  my_fwrite(&Send_offset[ThisTask], sizeof(int), 1, fd);
  my_fwrite(ids, sizeof(MyIDType), Nids, fd);

  fclose(fd);
}

int subfind_compare_ID_list(const void *a, const void *b)
{
  if(((struct id_list *) a)->GrNr < ((struct id_list *) b)->GrNr)
    return -1;

  if(((struct id_list *) a)->GrNr > ((struct id_list *) b)->GrNr)
    return +1;

  if(((struct id_list *) a)->SubNr < ((struct id_list *) b)->SubNr)
    return -1;

  if(((struct id_list *) a)->SubNr > ((struct id_list *) b)->SubNr)
    return +1;

  if(((struct id_list *) a)->BindingEgy < ((struct id_list *) b)->BindingEgy)
    return -1;

  if(((struct id_list *) a)->BindingEgy > ((struct id_list *) b)->BindingEgy)
    return +1;

  return 0;
}

int subfind_compare_SubGroup_GrNr_SubNr(const void *a, const void *b)
{
  if(((struct subgroup_properties *) a)->GrNr < ((struct subgroup_properties *) b)->GrNr)
    return -1;

  if(((struct subgroup_properties *) a)->GrNr > ((struct subgroup_properties *) b)->GrNr)
    return +1;

  if(((struct subgroup_properties *) a)->SubNr < ((struct subgroup_properties *) b)->SubNr)
    return -1;

  if(((struct subgroup_properties *) a)->SubNr > ((struct subgroup_properties *) b)->SubNr)
    return +1;

  return 0;
}

int subfind_compare_P_GrNr_DM_Density(const void *a, const void *b)
{
  if(((struct particle_data *) a)->GrNr < (((struct particle_data *) b)->GrNr))
    return -1;

  if(((struct particle_data *) a)->GrNr > (((struct particle_data *) b)->GrNr))
    return +1;

  if(((struct particle_data *) a)->u.DM_Density > (((struct particle_data *) b)->u.DM_Density))
    return -1;

  if(((struct particle_data *) a)->u.DM_Density < (((struct particle_data *) b)->u.DM_Density))
    return +1;

  return 0;
}

int subfind_compare_P_origindex(const void *a, const void *b)
{
  if(((struct particle_data *) a)->origindex < (((struct particle_data *) b)->origindex))
    return -1;

  if(((struct particle_data *) a)->origindex > (((struct particle_data *) b)->origindex))
    return +1;

  return 0;
}

#endif


#include "quantum_gates.h"
#include "quac_p.h"
#include <stdlib.h>
#include <stdio.h>
#include <petsc.h>
#include <stdarg.h>
#include "qasm_parser.h"
#include "error_correction.h"
#include "dm_utilities.h"
#include <ctype.h>

void quil_read(char filename[],PetscInt *num_qubits,circuit *circ){
  FILE *fp;
  int ch = 0,lines=0,finished_allocate=0,num_pragma=0,this_qubit,max_qubit=-1;
  char *line = NULL;
  size_t len = 0;
  ssize_t read;
  PetscReal time=1.0;

  fp = fopen(filename,"r");

  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_qasm_read!\n");
    }
  }

  //Count number of lines
  while(!feof(fp)){
    ch = fgetc(fp);
    if(ch == '\n'){
      lines++;
    }
  }
  //Rewind file
  rewind(fp);

  *num_qubits = 0;
  num_pragma  = 0;
  this_qubit  = -1;
  while ((read = getline(&line, &len, fp)) != -1){
    if (strstr(line,"PRAGMA")){
      //We will skip pragma lines
      num_pragma = num_pragma + 1;
    } else {
      this_qubit = line[strlen(line) - 2] - '0';
      if (this_qubit>max_qubit){
        max_qubit = this_qubit;
      }
    }
  }
  *num_qubits = max_qubit+1;

  lines = lines - num_pragma;

  create_circuit(circ,lines);
  rewind(fp); //Rewind the file (again)

  while ((read = getline(&line, &len, fp)) != -1){
    if (strstr(line,"PRAGMA")){
      //We will skip pragma lines
    } else if (strlen(line)>1){
      _quil_add_gate(line,circ,time);
      time = time + 1.0;
    }
  }

  fclose(fp);
  if (line) free(line);
  return;
}

void _quil_get_angle_pi(char angle_pi[32],PetscReal *angle){
  char numerator[32],denominator[32];
  int found_denom=1,i_n,i_d,i,denom,numer,factor;
  //Gate was printed with 'pi' or 'pi/2'
  angle_pi[strlen(angle_pi)-1] = 0;
  factor = 1;
  //Search for digits
  i_d = 0;   i_n = 0;
  denom = 1; numer = 1;
  for (i=0;angle_pi[i] != '\0'; i++){
    if (angle_pi[i]=='-'){
      //Found negative sign
      factor = -1;
    } else if (angle_pi[i]=="/") {
      // Found division; all digits after this
      // belong to the denominator
      found_denom = 1;
    } if (isdigit(angle_pi[i])){
      if (found_denom==0){
        //Numerator
        numerator[i_n] = angle_pi[i];
        i_n = i_n + 1;
      } else {
        //Denominator
        denominator[i_n] = angle_pi[i];
        i_d = i_d + 1;
      }
    }
  }
  if (i_d>0){
    denom = atoi(denominator);
  }
  if (i_n>0){
    numer = atoi(numerator);
  }
  *angle = factor * numer * PETSC_PI / denom;
  return;
}

void _quil_add_gate(char *line,circuit *circ,PetscReal time){
  char *token=NULL,*ptr=NULL;
  const char s[2] = " ";
  char angle_pi[32];
  int qubit1=-1,qubit2=-1;
  PetscReal angle;
  gate_type my_gate_type;
  // Split string on " " to separate the gate and the qubits
  while (token=strsep(&line," ")) {
    size_t i, j;
    for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
    //FIXME: Not exhaustive
    if (isdigit(token[0])){
      // check qubit numbers
      if (qubit1<0){
        qubit1 = atoi(token);
      } else {
        qubit2 = atoi(token);
      }
    } else {
      // gate types
      if (strcmp(token,"CNOT")==0){
        my_gate_type = CNOT;
      } else if (strcmp(token,"I")==0){
        my_gate_type = EYE;
      } else if (strcmp(token,"CZ")==0){
        my_gate_type = CZ;;
      } else if (strcmp(token,"H")==0) {
        my_gate_type = HADAMARD;
      } else if (strcmp(token,"Z")==0) {
        my_gate_type = SIGMAZ;
      } else if (strcmp(token,"X")==0) {
        my_gate_type = SIGMAX;
      } else if (strcmp(token,"Y")==0) {
        my_gate_type = SIGMAY;
      } else if (strstr(token,"RX")) {
        my_gate_type = RX;
        if (strstr(token,"pi")){
          sscanf(token,"RX(%s)",&angle_pi);
          _quil_get_angle_pi(angle_pi,&angle);
        } else {
          //Not sure if quil prints like this
          sscanf(token,"RX(%lf)",&angle);
        }
      } else if (strstr(token,"RY")) {
        my_gate_type = RY;
        if (strstr(token,"pi")){
          sscanf(token,"RY(%s)",&angle_pi);
          _quil_get_angle_pi(angle_pi,&angle);
        } else {
          //Not sure if quil prints like this
          sscanf(token,"RY(%lf)",&angle);
        }
      } else if (strstr(token,"RZ")) {
        if (strstr(token,"pi")){
          sscanf(token,"RZ(%s)",&angle_pi);
          _quil_get_angle_pi(angle_pi,&angle);
        } else {
          //Not sure if quil prints like this
          sscanf(token,"RZ(%lf)",&angle);
        }
        my_gate_type = RZ;
      }
    }
  }

  if (qubit2>0){
    //Multiqubit gate
    add_gate_to_circuit(circ,time,my_gate_type,qubit1,qubit2);
  } else {
    //Single qubit gate
    if (my_gate_type==6||my_gate_type==7||my_gate_type==8){
      //Rotation gate
      add_gate_to_circuit(circ,time,my_gate_type,qubit1,angle);
    } else {
      add_gate_to_circuit(circ,time,my_gate_type,qubit1);
    }
  }
  return;
}

void projectq_qasm_read(char filename[],PetscInt *num_qubits,circuit *circ){
  FILE *fp;
  int ch = 0,lines=0,finished_allocate=0;
  char *line = NULL;
  size_t len = 0;
  ssize_t read;
  PetscReal time=1.0;

  fp = fopen(filename,"r");

  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_qasm_read!\n");
    }
  }

  //Count number of lines
  while(!feof(fp)){
    ch = fgetc(fp);
    if(ch == '\n')
      {
        lines++;
      }
  }
  //Rewind file
  rewind(fp);

  *num_qubits = 0;
  while ((read = getline(&line, &len, fp)) != -1){
    if (!finished_allocate){
      if (strstr(line,"Allocate")){
        //Get number of qubits by reading number of lines with 'Allocate'
        *num_qubits = *num_qubits+1;
      } else {
        /*Subtract off the allocate lines to get the total number
         * of gates.
         * Factor of 2 because the end has DEALLOCATE statements
         * WARNING! Comments will be included, too - not too big of a problem,
         * just will overallocate a bit */
        lines = lines - 2*(*num_qubits);
        //Allocate the circuit
        create_circuit(circ,lines);
        finished_allocate = 1;
        //Add the first gate to list
        _projectq_qasm_add_gate(line,circ,time);
        time = time + 1.0;
        //        gate_list[0]
      }
    } else if (strstr(line,"Deallocate")){
      //Breakout since we've finished going through the circuit
      break;
    } else {
      _projectq_qasm_add_gate(line,circ,time);
      time = time + 1.0;
    }

  }

  fclose(fp);
  if (line) free(line);
  return;
}

void _projectq_qasm_add_gate(char *line,circuit *circ,PetscReal time){
  char *token=NULL;
  int qubit1,qubit2;
  PetscReal angle;
  gate_type my_gate_type;
  // Split string on | to separate gate type and qubits
  while (token=strsep(&line,"|")) {
    //Strip whitespace
    size_t i, j;
    for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
    //Do direct strcmp for some, strstr for others
    //FIXME: Not exhaustive
    if (strstr(token,"Qureg")){
      // qubit numbers
      if (my_gate_type<0){
        //Multiqubit gate
        sscanf(token,"(Qureg[%d],Qureg[%d])",&qubit1,&qubit2);
        add_gate_to_circuit(circ,time,my_gate_type,qubit1,qubit2);
      } else {
        //Single qubit gate
        sscanf(token,"Qureg[%d]",&qubit1);
        if (my_gate_type==6||my_gate_type==7||my_gate_type==8){
          //Rotation gate
          add_gate_to_circuit(circ,time,my_gate_type,qubit1,angle);
        } else {
          add_gate_to_circuit(circ,time,my_gate_type,qubit1);
        }
      }
    } else {
      // gate types
      if (strcmp(token,"CX")==0){
        my_gate_type = CNOT;
      } else if (strcmp(token,"H")==0) {
        my_gate_type = HADAMARD;
      } else if (strcmp(token,"Z")==0) {
        my_gate_type = SIGMAZ;
      } else if (strcmp(token,"X")==0) {
        my_gate_type = SIGMAX;
      } else if (strcmp(token,"Y")==0) {
        my_gate_type = SIGMAY;
      } else if (strstr(token,"Rx")) {
        my_gate_type = RX;
        sscanf(token,"Rx(%lf)",&angle);
      } else if (strstr(token,"Ry")) {
        my_gate_type = RY;
        sscanf(token,"Ry(%lf)",&angle);
      } else if (strstr(token,"Rz")) {
        sscanf(token,"Rz(%lf)",&angle);
        my_gate_type = RZ;
      }
    }
  }
}

void projectq_vqe_get_expectation(char filename[],Vec rho,PetscScalar *trace_val){
  FILE *fp;
  char *token=NULL,*token2=NULL;
  char *line = NULL,gate_char;
  size_t len = 0;
  ssize_t read;
  int token_number,num_ops,qubit_number;
  operator ops[100];
  PetscReal scalar_multiply;
  PetscScalar temp_trace_val;
  fp = fopen(filename,"r");
  *trace_val = 0.0;
  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_vqe_get_expectation!\n");
    }
  }

  while ((read = getline(&line, &len, fp)) != -1){
    token_number = 0;
    while (token=strsep(&line,"[")) {
      if(token_number==0){
        //Strip whitespace
        size_t i, j;
        for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
        scalar_multiply = atof(token);
        token_number = 1;
      } else {
        token2=strsep(&token,"]");
        num_ops = 0;
        while (token=strsep(&token2," ")) {
          if (strcmp(token,"")==0){
            *trace_val = *trace_val + scalar_multiply;
          } else {
            //Assume qubit number in file is global system number
            //FIXME: Put logical->physical qubit mapping here
            sscanf(token,"%c%d",&gate_char,&qubit_number);
            if (gate_char=='X'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_x;
            } else if (gate_char=='Y'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_y;
            } else if (gate_char=='Z'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_z;
            }
            num_ops = num_ops + 1;
          }
        }
        if (num_ops!=0){
          //This is a hack where I pass many ops, even though many of them
          //may not exist or be valid; they won't be accessed, at least.
          //Consider passing the array instead, and iterating inside?
          get_expectation_value(rho,&temp_trace_val,num_ops,
                                ops[0],ops[1],ops[2],ops[3],
                                ops[4],ops[5],ops[6],ops[7],
                                ops[8],ops[9],ops[10],ops[11],
                                ops[12],ops[13],ops[14],ops[15],
                                ops[16],ops[17],ops[18],ops[19]);
          temp_trace_val = temp_trace_val * scalar_multiply;
          *trace_val = *trace_val + temp_trace_val;
        }
      }
    }
  }
  return;
}

void projectq_vqe_get_expectation_squared(char filename[],Vec rho,PetscScalar *trace_val){
  FILE *fp;
  char *token=NULL,*token2=NULL;
  char *line = NULL,gate_char;
  size_t len = 0;
  ssize_t read;
  int token_number,num_ops,qubit_number;
  operator ops[100];
  PetscReal scalar_multiply;
  PetscScalar temp_trace_val;
  fp = fopen(filename,"r");
  *trace_val = 0.0;
  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_vqe_get_expectation!\n");
    }
  }

  while ((read = getline(&line, &len, fp)) != -1){
    token_number = 0;
    while (token=strsep(&line,"[")) {
      if(token_number==0){
        //Strip whitespace
        size_t i, j;
        for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
        scalar_multiply = atof(token);
        token_number = 1;
      } else {
        token2=strsep(&token,"]");
        num_ops = 0;
        while (token=strsep(&token2," ")) {
          if (strcmp(token,"")==0){
            *trace_val = *trace_val + scalar_multiply;
          } else {
            //Assume qubit number in file is global system number
            //FIXME: Put logical->physical qubit mapping here
            sscanf(token,"%c%d",&gate_char,&qubit_number);
            if (gate_char=='X'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_x;
            } else if (gate_char=='Y'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_y;
            } else if (gate_char=='Z'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_z;
            }
            num_ops = num_ops + 1;
          }
        }
        if (num_ops!=0){
          //This is a hack where I pass many ops, even though many of them
          //may not exist or be valid; they won't be accessed, at least.
          //Consider passing the array instead, and iterating inside?
          get_expectation_value(rho,&temp_trace_val,num_ops,
                                ops[0],ops[1],ops[2],ops[3],
                                ops[4],ops[5],ops[6],ops[7],
                                ops[8],ops[9],ops[10],ops[11],
                                ops[12],ops[13],ops[14],ops[15],
                                ops[16],ops[17],ops[18],ops[19]);
          temp_trace_val = temp_trace_val * scalar_multiply;
          *trace_val = *trace_val + temp_trace_val;
        }
      }
    }
  }
  return;
}

void projectq_vqe_get_expectation_encoded(char filename[],Vec rho,PetscScalar *trace_val,
                                          PetscInt num_encoders,...){
  FILE *fp;
  char *token=NULL,*token2=NULL;
  char *line = NULL,gate_char;
  size_t len = 0;
  ssize_t read;
  int token_number,num_ops,qubit_number;
  va_list ap;
  operator ops[100];
  PetscInt i;
  PetscReal scalar_multiply;
  PetscScalar temp_trace_val;
  encoded_qubit encoders[50]; //50 is more systems than we will be able to do

  va_start(ap,num_encoders);
  for (i=0;i<num_encoders;i++){
    encoders[i] = va_arg(ap,encoded_qubit);
  }

  fp = fopen(filename,"r");
  *trace_val = 0.0;
  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_vqe_get_expectation!\n");
    }
  }

  while ((read = getline(&line, &len, fp)) != -1){
    token_number = 0;
    while (token=strsep(&line,"[")) {
      if(token_number==0){
        //Strip whitespace
        size_t i, j;
        for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
        scalar_multiply = atof(token);
        token_number = 1;
      } else {
        token2=strsep(&token,"]");
        num_ops = 0;
        while (token=strsep(&token2," ")) {
          if (strcmp(token,"")==0){
            *trace_val = *trace_val + scalar_multiply;
          } else {
            //Assume qubit number in file is global system number
            //FIXME: Put logical->physical qubit mapping here
            sscanf(token,"%c%d",&gate_char,&qubit_number);
            qubit_number = encoders[qubit_number].qubits[0];
            if (gate_char=='X'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_x;
            } else if (gate_char=='Y'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_y;
            } else if (gate_char=='Z'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_z;
            }
            num_ops = num_ops + 1;
          }
        }
        if (num_ops!=0){
          if (num_ops>4){
            if (nid==0) {
              printf("ERROR! vqe_get_expectation only supports 4 ops for now\n");
              exit(0);
            }
          }
          get_expectation_value(rho,&temp_trace_val,num_ops,ops[0],ops[1],ops[2],ops[3]);
          temp_trace_val = temp_trace_val * scalar_multiply;
          *trace_val = *trace_val + temp_trace_val;
        }
      }
    }
  }
  return;
}


#include "imd.h"
#include <sys/time.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>

// #define USEFLOAT  // hauptsächlich in der funktion genexptint. Profiling zeigte, dass
                  // hier die meiste zeit verbraucht wird -> float verdoppelt performance

#ifdef USEFLOAT
typedef float Real;
#define REALTYPE MPI_FLOAT
#else
typedef double Real;
#define REALTYPE MPI_DOUBLE
#endif

#ifdef USEFLOAT
  #define EXPR     expf //exp zu floaten ist eine ganz mieeese idee
  #define SQRTR    sqrtf
  #define POWR     powf
  #define LOGR     logf
#else
  #define EXPR     exp
  #define SQRTR    sqrt
  #define POWR     pow
#define LOGR       log
#endif

// *********************************************************
// PHYSICAL CONSTANTS
// *********************************************************
// const double eV2J=1.6021766E-19;
const Real eV2H=0.03674932; //eV to Hartree
const Real colrad_reltol=1e-5;
const Real colrad_abstol=10.0;

// const Real J2eV=6.2415091E18;
const Real planck=6.62607004E-34; // J/s
const Real bohr_radius=0.52917721067E-10; // m
const Real bohr_radius_sq=2.800285202924816e-21;
const Real hbar_cub=1.172812163789953e-102; //hbar^3
const Real double_emass_pow_3_2 = 2.459112949719466e-45; // (2*emass)^3/2
const int MAXLINE = 255;
const Real  pi=3.141592653589793;
const Real  pi_sq=9.869604401089358;
const Real E_ion_H=13.6; // eV
const Real E_ion_H_J=2.178960176000000e-18; // J
const Real E_ion_H_sq_J=4.747867448593952e-36;

const Real colrad_tequi=1e-12;//TEST// 1e-12; //bei initial equi ohne Temperatur-variation erst einmal
                                //die Saha-besetzungsdichten equilibrieren

//const double  LIGHTSPEED=2.997925458e8; // m/s
Real  LASERFREQ;

int colrad_ydot(double t, N_Vector u, N_Vector udot, void *user_data);
void do_Saha(Real Te,Real totalc,Real ne,N_Vector y);
int colrad_GetCoeffs(N_Vector y,Real It, void * user_data);

// Die Zwei müssen nach Prototypes.h
// void do_colrad(double dt);
// void colrad_init(void);

void colrad_read_states(void);
void colrad_Saha_init(int i,int j,int k);

// ******************************************************************************
// *                CROSS SECTION INTEGRATION STUFF
// ******************************************************************************

gsl_integration_workspace * winteg_inner=NULL;
gsl_integration_workspace * winteg_outer=NULL;
gsl_integration_workspace * winteg_fermi=NULL;
gsl_integration_workspace * winteg_exc=NULL; //excitation

gsl_integration_romberg_workspace * winteg_rb_inner=NULL;
gsl_integration_romberg_workspace * winteg_rb_outer=NULL;

struct my_f_params { Real ne; Real T;Real mu; Real E;Real DeltaE; int allowed;};
// struct my_f_params fparams_inner; //For inner integrand
// struct my_f_params fparams_outer; //outer integrand
// struct my_f_params fparams_fermi;
// struct my_f_params fparams_exc;

double inner_integrand_ionization(double x, void *p); // integrate along E'
double outer_integrand_ionization(double x,void *p);  // integrate along E

Real double_integral_ionization(Real ne,Real T, Real mu, Real DeltaE); //evaluates double integral

double inner_integrand_recombination(double x, void *p);
double outer_integrand_recombination(double x,void *p);
Real double_integral_recombination(Real ne,Real T, Real mu, Real DeltaE);

double integrand_excitation(double x,void *p);
Real eval_excitation_integral(Real ne,Real T,Real mu, Real DeltaE, int allowed);

Real eval_dexcitation_integral(Real ne,Real T,Real mu, Real DeltaE, int allowed);
double integrand_deexcitation(double x,void *p);

double fermi_integrand(double x, void *p);
Real eval_fermi_integrand(Real ne,Real T, Real mu);

double integrand_excitation_debug(double x,void *p);

double outer_integrand_ionization2(double x,struct my_f_params* p);
Real double_integral_ionization2(Real ne,Real T, Real mu, Real DeltaE); //evaluates double integral
double inner_integrand_ionization2(double x, struct my_f_params* p);

// **********************************************************************************************
// *          PAR INTEGRAL STUFF
// **********************************************************************************************
int terminate_gkq;
int terminate_gkq_outer;
int terminate_gkq_inner;
int terminate_serial;
int gkq_iter_serial; // nr of iterations

const double gkq_alpha=0.816496580927726;
const double gkq_beta=0.447213595499958;
static const double xgkq[12] =
{
  0.0,
  -0.942882415695480,
  -0.816496580927726,
  -0.641853342345781,
  -0.447213595499958,
  -0.236383199662150,
  0.0,
  0.236383199662150,
  0.447213595499958,
  0.641853342345781,
  0.816496580927726,
  0.942882415695480
};

Real integral_simpson(Real (*f)(Real, void*), Real a, Real b,int n,void* p);

int simpson_error;
const Real tolmax=1e-20;
const Real simpson_itermax=120;
#define INITIAL_STACK_SIZE 128   /* initial size of new stacks */

/* the stack structure */
struct stack_s{
  int el_count;            /* count of elements on stack */
  int el_size;             /* size of an element */
  int mem_reserve;         /* allocated memory for stack */
  void* elements;          /* pointer to begin of stack */
};

typedef struct _work_t{
  double a;
  double b;
  double tol;
  double S;
  double fa;
  double fb;
  double fm;
  double rec;
  int iter;
  struct my_f_params * p; //pointer auf params
} work_t;

typedef struct _work_t_gkq{
  double a;
  double b;
  double toler;
  double I_13;
  double I_prev;
  double fa;
  double fb;
  struct my_f_params * p; //pointer auf params
  shortint iter;
} work_gkq;

typedef struct stack_s* stack_t;
double integral_simpson_par(double (*f)(double, struct my_f_params*), stack_t stack);

double gkq_adapt_OMP(double (*f)(double, struct my_f_params*), stack_t stack);
double gkq_OMP(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p,stack_t stack);

double gkq_serial(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p);
double gkq_adapt_serial(double (*f)(double, struct my_f_params*), double a, double b,
                        double fa,double fb, double toler,double I_13, struct my_f_params* p);

// void create_stack(stack_t* stack, int element_size);
// int  empty_stack(stack_t stack);
// void push_stack(stack_t stack, void* element);
// void pop_stack(stack_t stack, void* element);

/******************************************
 * create new stack
 ******************************************/
void create_stack(
             stack_t* stack,     /* stack to create */
             int element_size)   /* size of a stack element */
{
  int initial_size = INITIAL_STACK_SIZE;

  /* allocate memory for new stack struct */
  (*stack) = (stack_t) malloc(sizeof(struct stack_s));
  if (!(*stack)){
    char errstr[255];
    sprintf(errstr, "error: could not allocate memory for stack.. Abort.\n");
    error(errstr);
    // exit(1);
  }

  /* allocate memory for stack elements */
  (*stack)->elements = (void*) malloc(element_size * initial_size);
  (*stack)->mem_reserve = initial_size;
  if (!(*stack)->elements){
    char errstr[255];
    sprintf(errstr, "error: could not allocate memory for stack.. Abort.\n");
    error(errstr);
  }

  (*stack)->el_size = element_size;
  (*stack)->el_count = 0;

}

/*****************************************
 * check if the stack is empty
 *****************************************/
int empty_stack(stack_t stack)
{
  return stack->el_count <= 0;
}

/*****************************************
 * push a element on stack
 *****************************************/
void push_stack(stack_t stack,    /* target stack */
           void* element)    /* element to push */
{
  int i, new_reserve;
  int log2_count;

  /* check if we need more memory for stack */
  if (stack->el_count >= stack->mem_reserve)
  {

      /* calculate new size for the stack
         it should be a power of two */
      for (i = stack->el_count, log2_count = 0;
           i > 0;
           i>>1, log2_count++);
      new_reserve = 1 << log2_count;

      /* reallocate memory for phase thread tables
         and nullify new values */
      stack->elements = (void *) realloc(stack->elements,
                      stack->el_size * new_reserve);
      if (!stack->elements){
        char errstr [255];
        sprintf(errstr, "error: can't reallocate stack.. Aborting\n");
        error(errstr);
        // exit(1);
      }

      stack->mem_reserve = new_reserve;
  }

  /* now push the element on top of the stack */
  memcpy((char*)stack->elements + stack->el_count*stack->el_size,
            element, stack->el_size);
  stack->el_count++;

}

/*****************************************
 * pop an element from stack
 *****************************************/
void pop_stack(
          stack_t stack,    /* target stack */
          void* element)    /* where poped el. should be stored */
{
  if (stack->el_count <= 0){
    char errstr[255];
    sprintf(errstr, "error: trying to pop from empty stack.\n");
    error(errstr);
    // exit(2);
  }

  stack->el_count--;
  memcpy(element,
          (char*)stack->elements + stack->el_count*stack->el_size,
          stack->el_size);

}

// ***************************************************************************
// *      Gauss-kronard quadrature, parallel
// ***************************************************************************
double gkq_OMP(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p,stack_t stack)
{
  //1st integration
  double result=0.0;
// *********************************************

  double m=0.5*(a+b);
  double h=0.5*(b-a);
  double y[13];
  double fa=y[0]=f(a,p);
  double fb=y[12]=f(b,p);
  int i;
  for(i=1;i<12;i++)
    y[i]=f(m+xgkq[i]*h,p);

  double I_4= (h/6.0)*(y[0]+y[12]+5.0*(y[4]+y[8]));  // 4-point gauss-lobatto
  double I_7= (h/1470.0)*(77.0*(y[0]+y[12])+432.0*(y[2]+y[10])+  // 7-point kronrod
               625.0*(y[4]+y[8])+672.0*y[6]);

  double I_13= h*(0.0158271919734802*(y[0]+y[12])+0.0942738402188500*(y[1]+y[11])+0.155071987336585*(y[2]+y[10])+
                  0.188821573960182*(y[3]+y[9])+0.199773405226859*(y[4]+y[8])+0.224926465333340*(y[5]+y[7])+
                  0.242611071901408*y[6]);   //13-point Kronrod

  double Err1=fabs(I_7-I_13);
  double Err2=fabs(I_4-I_13);

  double r=(Err2 != 0.0) ? Err1/Err2 : 1.0;
  double toler=(r > 0.0 && r < 1.0) ? TOL/r : TOL;

  if(I_13 == 0)
    I_13=b-a;
  I_13=fabs(I_13);

  //Prepare work and push onto stack
  work_gkq work;

  work.a = a;
  work.b = b;
  work.toler = toler;
  work.I_13=I_13;
  work.fa=fa;
  work.fb=fb;
  work.p=p;
  work.I_prev=I_7;

  //ANTI-FOLGENDES:
  //OUT OF TOLERANCE !!!, mll:3.0162e-18, a:3.0162e-18, b:3.0162e-18, mrr:3.0162e-18,I_7-I_4:0.0000e+00, tol:1.6002e-315,I_13:7.0585e-313
  if(I_13 < 1e-150)
    return 0;

  push_stack(stack, &work);
  result=gkq_adapt(f,stack);
  return result;
}

double gkq_serial(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p)
{
  //1st integration

  double result=0.0;
  gkq_iter_serial=0;
// *********************************************

  double m=0.5*(a+b);
  double h=0.5*(b-a);
  double y[13];
  double fa=y[0]=f(a,p);
  double fb=y[12]=f(b,p);
  int i;
  for(i=1;i<12;i++)
    y[i]=f(m+xgkq[i]*h,p);

  double I_4= (h/6.0)*(y[0]+y[12]+5.0*(y[4]+y[8]));  // 4-point gauss-lobatto
  double I_7= (h/1470.0)*(77.0*(y[0]+y[12])+432.0*(y[2]+y[10])+  // 7-point kronrod
               625.0*(y[4]+y[8])+672.0*y[6]);

  double I_13= h*(0.0158271919734802*(y[0]+y[12])+0.0942738402188500*(y[1]+y[11])+0.155071987336585*(y[2]+y[10])+
                  0.188821573960182*(y[3]+y[9])+0.199773405226859*(y[4]+y[8])+0.224926465333340*(y[5]+y[7])+
                  0.242611071901408*y[6]);   //13-point Kronrod

  double Err1=fabs(I_7-I_13);
  double Err2=fabs(I_4-I_13);

  double r=(Err2 != 0.0) ? Err1/Err2 : 1.0;
  double toler=(r > 0.0 && r < 1.0) ? TOL/r : TOL;

  if(I_13 == 0)
    I_13=b-a;
  I_13=fabs(I_13);

  result=gkq_adapt_serial(f,a,b,fa,fb,toler,I_13, p);

  return result;
}

// ***********************************************
// * RECURSIVE ADAPTION ROUTINE FOR PARALLEL-GK-QUADRATURE
// **********************************************
double gkq_adapt_OMP(double (*f)(double, struct my_f_params*), stack_t stack)
{
  work_gkq work;
  work.iter=0;
  int ready, idle, busy;
  double integral_result = 0.0;

  busy = 0;
  terminate_gkq=0;

#pragma omp parallel default(none) \
    shared(stack, integral_result,f,busy,terminate_gkq,myid) \
    private(work, idle, ready)
  {
// printf("me:%d, err:%d\n",omp_get_thread_num(),simpson_error);

    ready = 0;
    idle = 1;

    while(!ready) // && !terminate_gkq)//  && !simpson_error) //<-- so NICHT!
    {
      #pragma omp critical (stack)
      {
        if (!empty_stack(stack))
        {
          /* we have new work */
          pop_stack(stack, &work);
          if (idle)
          {
            /* say others i'm busy */
            busy += 1;
            idle = 0;
          }
        }
        else
        {
          /* no new work on stack */
          if (!idle){
            busy -= 1;
            idle = 1;
          }

          /* nobody has anything to do; let us leave the loop */
          if (busy == 0)
          {
            ready = 1;
          }
        }
      } /* end critical(stack) */

      if (idle)
        continue; //if ready==1 --> leave loop
double I_prev=work.I_prev;

      double a = work.a;
      double b = work.b;
      double toler = work.toler;
      double I_13=work.I_13;
      double fa=work.fa;
      double fb=work.fb;
      int iter=work.iter;
      // double *y= work.y; // brauch ich nicht!
      struct my_f_params * p = work.p;

      double m = (a+b)/2;
      double h = (b -a)/2;
      double mll=m-gkq_alpha*h;
      double ml=m-gkq_beta*h;
      double mr=m+gkq_beta*h;
      double mrr=m+gkq_alpha*h;

      double fmll=f(mll,p);
      double fml=f(ml,p);
      double fm=f(m,p);
      double fmr=f(mr,p);
      double fmrr=f(mrr,p);
      double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
      double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

// if(myid==1)
//   printf("I_7:%.4e, I_13:%.4e,I_4:%.4e, minus:%.4e, to:%.4e\n",I_7,I_13,I_4,I_7-I_4, toler*I_13);
int maxiter=50; //max. subdivisions
double abstol=1e-30;
work.I_prev=I_7; // für abstolcheck in nächster recursion

      if (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr || iter > maxiter || fabs(I_7-I_prev) < abstol )
      {
        if ((mll <= a || b <= mrr)) //Error
         {
           // out_of_tolerance=true; // Interval contains no more machine numbers
           // printf("OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e,I_7-I_4:%.4e, tol:%.4e,I_13:%.4e\n",
           //        mll,b,b,mrr,I_7-I_4, toler*I_13,I_13);
           terminate_gkq=1;
         }
        #pragma omp critical (integral_result)
        {
          integral_result += I_7;      //Terminate recursion.
        }
// printf("me ok:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f,ubteg;%.4e\n", omp_get_thread_num(), a,b,toler,I_4,I_7,integral_result);

      }
      else  //subdivide interval and push new work on stack
      {
        #pragma omp critical (stack)
        {

          // printf("me NOOOO:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);

          work.iter=iter+1;

          work.a=a;
          work.b=mll;
          work.fa=fa;
          work.fb=fmll;
          push_stack(stack, &work);

          work.a=mll;
          work.b=ml;
          work.fa=fmll;
          work.fb=fml;
          push_stack(stack, &work);

          work.a=ml;
          work.b=m;
          work.fa=fml;
          work.fb=fm;
          push_stack(stack, &work);

          work.a=m;
          work.b=mr;
          work.fa=fm;
          work.fb=fmr;
          push_stack(stack, &work);

          work.a=mr;
          work.b=mrr;
          work.fa=fmr;
          work.fb=fmrr;
          push_stack(stack, &work);

          work.a=mrr;
          work.b=b;
          work.fa=fmrr;
          work.fb=fb;
          push_stack(stack, &work);

        } // pragma critical stack
      }   // else ..non-acceptable error
    } // while
  } /* end omp parallel */
  return integral_result;
}

double gkq_adapt_serial(double (*f)(double, struct my_f_params*), double a, double b, double fa,
                        double fb, double toler,double I_13, struct my_f_params* p)
{
  double m = (a+b)/2;
  double h = (b -a)/2;
  double mll=m-gkq_alpha*h;
  double ml=m-gkq_beta*h;
  double mr=m+gkq_beta*h;
  double mrr=m+gkq_alpha*h;

  double fmll=f(mll,p);
  double fml=f(ml,p);
  double fm=f(m,p);
  double fmr=f(mr,p);
  double fmrr=f(mrr,p);
  double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
  double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

  gkq_iter_serial++;

  if ( (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr) && gkq_iter_serial)
  {
    if ((mll <= a || b <= mrr) && !terminate_serial) //Error
     {
      // out_of_tolerance=true; // Interval contains no more machine numbers
      printf("OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e\n", mll,b,b,mrr);
      terminate_serial=1;
     }

// printf("me ok:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);
    return I_7;
  }
  else
  {

    // printf("me NOOOO:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);

    return  gkq_adapt_serial(f, a,mll,fa,fmll,toler,I_13,p) +
            gkq_adapt_serial(f, mll,ml,fmll,fml,toler,I_13,p) +
            gkq_adapt_serial(f, ml,m,fml,fm,toler,I_13,p) +
            gkq_adapt_serial(f, m,mr,fm,fmr,toler,I_13,p) +
            gkq_adapt_serial(f, mr,mrr,fmr,fmrr,toler,I_13,p) +
            gkq_adapt_serial(f, mrr,b,fmrr,fb,toler,I_13,p);
  }

}


/* specfunc/airy.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_airy.h>

#include "error.h"
#include "check.h"

#include "chebyshev.h"
#include "cheb_eval_mode.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* chebyshev expansions for Airy modulus and phase
   based on SLATEC r9aimp()

 Series for AM21       on the interval -1.25000D-01 to  0.
                                        with weighted error   2.89E-17
                                         log weighted error  16.54
                               significant figures required  14.15
                                    decimal places required  17.34

 Series for ATH1       on the interval -1.25000D-01 to  0.
                                        with weighted error   2.53E-17
                                         log weighted error  16.60
                               significant figures required  15.15
                                    decimal places required  17.38

 Series for AM22       on the interval -1.00000D+00 to -1.25000D-01
                                        with weighted error   2.99E-17
                                         log weighted error  16.52
                               significant figures required  14.57
                                    decimal places required  17.28

 Series for ATH2       on the interval -1.00000D+00 to -1.25000D-01
                                        with weighted error   2.57E-17
                                         log weighted error  16.59
                               significant figures required  15.07
                                    decimal places required  17.34
*/

static double am21_data[37] = {
  0.0065809191761485,
  0.0023675984685722,
  0.0001324741670371,
  0.0000157600904043,
  0.0000027529702663,
  0.0000006102679017,
  0.0000001595088468,
  0.0000000471033947,
  0.0000000152933871,
  0.0000000053590722,
  0.0000000020000910,
  0.0000000007872292,
  0.0000000003243103,
  0.0000000001390106,
  0.0000000000617011,
  0.0000000000282491,
  0.0000000000132979,
  0.0000000000064188,
  0.0000000000031697,
  0.0000000000015981,
  0.0000000000008213,
  0.0000000000004296,
  0.0000000000002284,
  0.0000000000001232,
  0.0000000000000675,
  0.0000000000000374,
  0.0000000000000210,
  0.0000000000000119,
  0.0000000000000068,
  0.0000000000000039,
  0.0000000000000023,
  0.0000000000000013,
  0.0000000000000008,
  0.0000000000000005,
  0.0000000000000003,
  0.0000000000000001,
  0.0000000000000001
};
static cheb_series am21_cs = {
  am21_data,
  36,
  -1, 1,
  20
};

static double ath1_data[36] = {
  -0.07125837815669365,
  -0.00590471979831451,
  -0.00012114544069499,
  -0.00000988608542270,
  -0.00000138084097352,
  -0.00000026142640172,
  -0.00000006050432589,
  -0.00000001618436223,
  -0.00000000483464911,
  -0.00000000157655272,
  -0.00000000055231518,
  -0.00000000020545441,
  -0.00000000008043412,
  -0.00000000003291252,
  -0.00000000001399875,
  -0.00000000000616151,
  -0.00000000000279614,
  -0.00000000000130428,
  -0.00000000000062373,
  -0.00000000000030512,
  -0.00000000000015239,
  -0.00000000000007758,
  -0.00000000000004020,
  -0.00000000000002117,
  -0.00000000000001132,
  -0.00000000000000614,
  -0.00000000000000337,
  -0.00000000000000188,
  -0.00000000000000105,
  -0.00000000000000060,
  -0.00000000000000034,
  -0.00000000000000020,
  -0.00000000000000011,
  -0.00000000000000007,
  -0.00000000000000004,
  -0.00000000000000002
};
static cheb_series ath1_cs = {
  ath1_data,
  35,
  -1, 1,
  15
};

static double am22_data[33] = {
 -0.01562844480625341,
  0.00778336445239681,
  0.00086705777047718,
  0.00015696627315611,
  0.00003563962571432,
  0.00000924598335425,
  0.00000262110161850,
  0.00000079188221651,
  0.00000025104152792,
  0.00000008265223206,
  0.00000002805711662,
  0.00000000976821090,
  0.00000000347407923,
  0.00000000125828132,
  0.00000000046298826,
  0.00000000017272825,
  0.00000000006523192,
  0.00000000002490471,
  0.00000000000960156,
  0.00000000000373448,
  0.00000000000146417,
  0.00000000000057826,
  0.00000000000022991,
  0.00000000000009197,
  0.00000000000003700,
  0.00000000000001496,
  0.00000000000000608,
  0.00000000000000248,
  0.00000000000000101,
  0.00000000000000041,
  0.00000000000000017,
  0.00000000000000007,
  0.00000000000000002
};
static cheb_series am22_cs = {
  am22_data,
  32,
  -1, 1,
  15
};

static double ath2_data[32] = {
   0.00440527345871877,
  -0.03042919452318455,
  -0.00138565328377179,
  -0.00018044439089549,
  -0.00003380847108327,
  -0.00000767818353522,
  -0.00000196783944371,
  -0.00000054837271158,
  -0.00000016254615505,
  -0.00000005053049981,
  -0.00000001631580701,
  -0.00000000543420411,
  -0.00000000185739855,
  -0.00000000064895120,
  -0.00000000023105948,
  -0.00000000008363282,
  -0.00000000003071196,
  -0.00000000001142367,
  -0.00000000000429811,
  -0.00000000000163389,
  -0.00000000000062693,
  -0.00000000000024260,
  -0.00000000000009461,
  -0.00000000000003716,
  -0.00000000000001469,
  -0.00000000000000584,
  -0.00000000000000233,
  -0.00000000000000093,
  -0.00000000000000037,
  -0.00000000000000015,
  -0.00000000000000006,
  -0.00000000000000002
};
static cheb_series ath2_cs = {
  ath2_data,
  31,
  -1, 1,
  16
};

/* Airy modulus and phase for x < -1 */
static
int
airy_mod_phase(const double x, gsl_mode_t mode, gsl_sf_result * mod, gsl_sf_result * phase)
{
  gsl_sf_result result_m;
  gsl_sf_result result_p;
  double m, p;
  double sqx;

  if(x < -2.0) {
    double z = 16.0/(x*x*x) + 1.0;
    cheb_eval_mode_e(&am21_cs, z, mode, &result_m);
    cheb_eval_mode_e(&ath1_cs, z, mode, &result_p);
  }
  else if(x <= -1.0) {
    double z = (16.0/(x*x*x) + 9.0)/7.0;
    cheb_eval_mode_e(&am22_cs, z, mode, &result_m);
    cheb_eval_mode_e(&ath2_cs, z, mode, &result_p);
  }
  else {
    mod->val = 0.0;
    mod->err = 0.0;
    phase->val = 0.0;
    phase->err = 0.0;
    GSL_ERROR ("x is greater than 1.0", GSL_EDOM);
  }

  m =  0.3125 + result_m.val;
  p = -0.625  + result_p.val;

  sqx = sqrt(-x);

  mod->val   = sqrt(m/sqx);
  mod->err  = fabs(mod->val) * (GSL_DBL_EPSILON + fabs(result_m.err/result_m.val));
  phase->val = M_PI_4 - x*sqx * p;
  phase->err = fabs(phase->val) * (GSL_DBL_EPSILON + fabs(result_p.err/result_p.val));

  return GSL_SUCCESS;
}

/* Chebyshev series for Ai(x) with x in [-1,1]
   based on SLATEC ai(x)

 series for aif        on the interval -1.00000d+00 to  1.00000d+00
                                   with weighted error   1.09e-19
                                    log weighted error  18.96
                          significant figures required  17.76
                               decimal places required  19.44

 series for aig        on the interval -1.00000d+00 to  1.00000d+00
                                   with weighted error   1.51e-17
                                    log weighted error  16.82
                          significant figures required  15.19
                               decimal places required  17.27
 */
static double ai_data_f[9] = {
  -0.03797135849666999750,
   0.05919188853726363857,
   0.00098629280577279975,
   0.00000684884381907656,
   0.00000002594202596219,
   0.00000000006176612774,
   0.00000000000010092454,
   0.00000000000000012014,
   0.00000000000000000010
};
static cheb_series aif_cs = {
  ai_data_f,
  8,
  -1, 1,
  8
};

static double ai_data_g[8] = {
   0.01815236558116127,
   0.02157256316601076,
   0.00025678356987483,
   0.00000142652141197,
   0.00000000457211492,
   0.00000000000952517,
   0.00000000000001392,
   0.00000000000000001
};
static cheb_series aig_cs = {
  ai_data_g,
  7,
  -1, 1,
  7
};

/* Chebvyshev series for Bi(x) with x in [-1,1]
   based on SLATEC bi(x)

 series for bif        on the interval -1.00000d+00 to  1.00000d+00
                                        with weighted error   1.88e-19
                                         log weighted error  18.72
                               significant figures required  17.74
                                    decimal places required  19.20

 series for big        on the interval -1.00000d+00 to  1.00000d+00
                                        with weighted error   2.61e-17
                                         log weighted error  16.58
                               significant figures required  15.17
                                    decimal places required  17.03
 */
static double data_bif[9] = {
  -0.01673021647198664948,
   0.10252335834249445610,
   0.00170830925073815165,
   0.00001186254546774468,
   0.00000004493290701779,
   0.00000000010698207143,
   0.00000000000017480643,
   0.00000000000000020810,
   0.00000000000000000018
};
static cheb_series bif_cs = {
  data_bif,
  8,
  -1, 1,
  8
};

static double data_big[8] = {
   0.02246622324857452,
   0.03736477545301955,
   0.00044476218957212,
   0.00000247080756363,
   0.00000000791913533,
   0.00000000001649807,
   0.00000000000002411,
   0.00000000000000002
};
static cheb_series big_cs = {
  data_big,
  7,
  -1, 1,
  7
};

/* Chebyshev series for Bi(x) with x in [1,8]
   based on SLATEC bi(x)
 */
static double data_bif2[10] = {
  0.0998457269381604100,
  0.4786249778630055380,
  0.0251552119604330118,
  0.0005820693885232645,
  0.0000074997659644377,
  0.0000000613460287034,
  0.0000000003462753885,
  0.0000000000014288910,
  0.0000000000000044962,
  0.0000000000000000111
};
static cheb_series bif2_cs = {
  data_bif2,
  9,
  -1, 1,
  9
};

static double data_big2[10] = {
  0.033305662145514340,
  0.161309215123197068,
  0.0063190073096134286,
  0.0001187904568162517,
  0.0000013045345886200,
  0.0000000093741259955,
  0.0000000000474580188,
  0.0000000000001783107,
  0.0000000000000005167,
  0.0000000000000000011
};
static cheb_series big2_cs = {
  data_big2,
  9,
  -1, 1,
  9
};

/* chebyshev for Ai(x) asymptotic factor
   based on SLATEC aie()

 Series for AIP        on the interval  0.          to  1.00000D+00
                   with weighted error   5.10E-17
                    log weighted error  16.29
          significant figures required  14.41
               decimal places required  17.06

 [GJ] Sun Apr 19 18:14:31 EDT 1998
 There was something wrong with these coefficients. I was getting
 errors after 3 or 4 digits. So I recomputed this table. Now I get
 double precision agreement with Mathematica. But it does not seem
 possible that the small differences here would account for the
 original discrepancy. There must have been something wrong with my
 original usage...
*/
static double data_aip[36] = {
 -0.0187519297793867540198,
 -0.0091443848250055004725,
  0.0009010457337825074652,
 -0.0001394184127221491507,
  0.0000273815815785209370,
 -0.0000062750421119959424,
  0.0000016064844184831521,
 -0.0000004476392158510354,
  0.0000001334635874651668,
 -0.0000000420735334263215,
  0.0000000139021990246364,
 -0.0000000047831848068048,
  0.0000000017047897907465,
 -0.0000000006268389576018,
  0.0000000002369824276612,
 -0.0000000000918641139267,
  0.0000000000364278543037,
 -0.0000000000147475551725,
  0.0000000000060851006556,
 -0.0000000000025552772234,
  0.0000000000010906187250,
 -0.0000000000004725870319,
  0.0000000000002076969064,
 -0.0000000000000924976214,
  0.0000000000000417096723,
 -0.0000000000000190299093,
  0.0000000000000087790676,
 -0.0000000000000040927557,
  0.0000000000000019271068,
 -0.0000000000000009160199,
  0.0000000000000004393567,
 -0.0000000000000002125503,
  0.0000000000000001036735,
 -0.0000000000000000509642,
  0.0000000000000000252377,
 -0.0000000000000000125793
/*
  -.0187519297793868
  -.0091443848250055,
   .0009010457337825,
  -.0001394184127221,
   .0000273815815785,
  -.0000062750421119,
   .0000016064844184,
  -.0000004476392158,
   .0000001334635874,
  -.0000000420735334,
   .0000000139021990,
  -.0000000047831848,
   .0000000017047897,
  -.0000000006268389,
   .0000000002369824,
  -.0000000000918641,
   .0000000000364278,
  -.0000000000147475,
   .0000000000060851,
  -.0000000000025552,
   .0000000000010906,
  -.0000000000004725,
   .0000000000002076,
  -.0000000000000924,
   .0000000000000417,
  -.0000000000000190,
   .0000000000000087,
  -.0000000000000040,
   .0000000000000019,
  -.0000000000000009,
   .0000000000000004,
  -.0000000000000002,
   .0000000000000001,
  -.0000000000000000
*/
};
static cheb_series aip_cs = {
  data_aip,
  35,
  -1, 1,
  17
};

/* chebyshev for Bi(x) asymptotic factor
   based on SLATEC bie()

 Series for BIP        on the interval  1.25000D-01 to  3.53553D-01
                   with weighted error   1.91E-17
                    log weighted error  16.72
          significant figures required  15.35
               decimal places required  17.41

 Series for BIP2       on the interval  0.          to  1.25000D-01
                   with weighted error   1.05E-18
                    log weighted error  17.98
          significant figures required  16.74
               decimal places required  18.71
*/
static double data_bip[24] = {
  -0.08322047477943447,
   0.01146118927371174,
   0.00042896440718911,
  -0.00014906639379950,
  -0.00001307659726787,
   0.00000632759839610,
  -0.00000042226696982,
  -0.00000019147186298,
   0.00000006453106284,
  -0.00000000784485467,
  -0.00000000096077216,
   0.00000000070004713,
  -0.00000000017731789,
   0.00000000002272089,
   0.00000000000165404,
  -0.00000000000185171,
   0.00000000000059576,
  -0.00000000000012194,
   0.00000000000001334,
   0.00000000000000172,
  -0.00000000000000145,
   0.00000000000000049,
  -0.00000000000000011,
   0.00000000000000001
};
static cheb_series bip_cs = {
  data_bip,
  23,
  -1, 1,
  14
};

static double data_bip2[29] = {
  -0.113596737585988679,
   0.0041381473947881595,
   0.0001353470622119332,
   0.0000104273166530153,
   0.0000013474954767849,
   0.0000001696537405438,
  -0.0000000100965008656,
  -0.0000000167291194937,
  -0.0000000045815364485,
   0.0000000003736681366,
   0.0000000005766930320,
   0.0000000000621812650,
  -0.0000000000632941202,
  -0.0000000000149150479,
   0.0000000000078896213,
   0.0000000000024960513,
  -0.0000000000012130075,
  -0.0000000000003740493,
   0.0000000000002237727,
   0.0000000000000474902,
  -0.0000000000000452616,
  -0.0000000000000030172,
   0.0000000000000091058,
  -0.0000000000000009814,
  -0.0000000000000016429,
   0.0000000000000005533,
   0.0000000000000002175,
  -0.0000000000000001737,
  -0.0000000000000000010
};
static cheb_series bip2_cs = {
  data_bip2,
  28,
  -1, 1,
  10
};

/* assumes x >= 1.0 */
inline static int
airy_aie(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  double sqx = sqrt(x);
  double z   = 2.0/(x*sqx) - 1.0;
  double y   = sqrt(sqx);
  gsl_sf_result result_c;
  cheb_eval_mode_e(&aip_cs, z, mode, &result_c);
  result->val = (0.28125 + result_c.val)/y;
  result->err = result_c.err/y + GSL_DBL_EPSILON * fabs(result->val);
  return GSL_SUCCESS;
}

/* assumes x >= 2.0 */
static int airy_bie(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  const double ATR =  8.7506905708484345;
  const double BTR = -2.0938363213560543;

  if(x < 4.0) {
    double sqx = sqrt(x);
    double z   = ATR/(x*sqx) + BTR;
    double y   = sqrt(sqx);
    gsl_sf_result result_c;
    cheb_eval_mode_e(&bip_cs, z, mode, &result_c);
    result->val = (0.625 + result_c.val)/y;
    result->err = result_c.err/y + GSL_DBL_EPSILON * fabs(result->val);
  }
  else {
    double sqx = sqrt(x);
    double z   = 16.0/(x*sqx) - 1.0;
    double y   = sqrt(sqx);
    gsl_sf_result result_c;
    cheb_eval_mode_e(&bip2_cs, z, mode, &result_c);
    result->val = (0.625 + result_c.val)/y;
    result->err = result_c.err/y + GSL_DBL_EPSILON * fabs(result->val);
  }

  return GSL_SUCCESS;
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_airy_Ai_e(const double x, const gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result cos_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_cos = gsl_sf_cos_err_e(theta.val, theta.err, &cos_result);
    result->val  = mod.val * cos_result.val;
    result->err  = fabs(mod.val * cos_result.err) + fabs(cos_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_cos);
  }
  else if(x <= 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&aif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&aig_cs, z, mode, &result_c1);
    result->val  = 0.375 + (result_c0.val - x*(0.25 + result_c1.val));
    result->err  = result_c0.err + fabs(x*result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    double x32 = x * sqrt(x);
    double s   = exp(-2.0*x32/3.0);
    gsl_sf_result result_aie;
    int stat_aie = airy_aie(x, mode, &result_aie);
    result->val  = result_aie.val * s;
    result->err  = result_aie.err * s + result->val * x32 * GSL_DBL_EPSILON;
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    CHECK_UNDERFLOW(result);
    return stat_aie;
  }
}

int
gsl_sf_airy_Ai_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result cos_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_cos = gsl_sf_cos_err_e(theta.val, theta.err, &cos_result);
    result->val  = mod.val * cos_result.val;
    result->err  = fabs(mod.val * cos_result.err) + fabs(cos_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_cos);
  }
  else if(x <= 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&aif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&aig_cs, z, mode, &result_c1);
    result->val  = 0.375 + (result_c0.val - x*(0.25 + result_c1.val));
    result->err  = result_c0.err + fabs(x*result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);

    if(x > 0.0) {
      const double scale = exp(2.0/3.0 * sqrt(z));
      result->val *= scale;
      result->err *= scale;
    }

    return GSL_SUCCESS;
  }
  else {
    return airy_aie(x, mode, result);
  }
}

int gsl_sf_airy_Bi_e(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */
  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result sin_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_sin = gsl_sf_sin_err_e(theta.val, theta.err, &sin_result);
    result->val  = mod.val * sin_result.val;
    result->err  = fabs(mod.val * sin_result.err) + fabs(sin_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_sin);
  }
  else if(x < 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big_cs, z, mode, &result_c1);
    result->val  = 0.625 + result_c0.val + x*(0.4375 + result_c1.val);
    result->err  = result_c0.err + fabs(x * result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= 2.0) {
    const double z = (2.0*x*x*x - 9.0)/7.0;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif2_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big2_cs, z, mode, &result_c1);
    result->val  = 1.125 + result_c0.val + x*(0.625 + result_c1.val);
    result->err  = result_c0.err + fabs(x * result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double y = 2.0*x*sqrt(x)/3.0;
    const double s = exp(y);

    if(y > GSL_LOG_DBL_MAX - 1.0) {
      OVERFLOW_ERROR(result);
    }
    else {
      gsl_sf_result result_bie;
      int stat_bie = airy_bie(x, mode, &result_bie);
      result->val  = result_bie.val * s;
      result->err  = result_bie.err * s + fabs(1.5*y * (GSL_DBL_EPSILON * result->val));
      result->err += GSL_DBL_EPSILON * fabs(result->val);
      return stat_bie;
    }
  }
}

int
gsl_sf_airy_Bi_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result sin_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_sin = gsl_sf_sin_err_e(theta.val, theta.err, &sin_result);
    result->val  = mod.val * sin_result.val;
    result->err  = fabs(mod.val * sin_result.err) + fabs(sin_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_sin);
  }
  else if(x < 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big_cs, z, mode, &result_c1);
    result->val  = 0.625 + result_c0.val + x*(0.4375 + result_c1.val);
    result->err  = result_c0.err + fabs(x * result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    if(x > 0.0) {
      const double scale = exp(-2.0/3.0 * sqrt(z));
      result->val *= scale;
      result->err *= scale;
    }
    return GSL_SUCCESS;
  }
  else if(x <= 2.0) {
    const double x3 = x*x*x;
    const double z  = (2.0*x3 - 9.0)/7.0;
    const double s  = exp(-2.0/3.0 * sqrt(x3));
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif2_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big2_cs, z, mode, &result_c1);
    result->val  = s * (1.125 + result_c0.val + x*(0.625 + result_c1.val));
    result->err  = s * (result_c0.err + fabs(x * result_c1.err));
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    return airy_bie(x, mode, result);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_airy_Ai(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Ai_e(x, mode, &result));
}

double gsl_sf_airy_Ai_scaled(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Ai_scaled_e(x, mode, &result));
}

double gsl_sf_airy_Bi(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Bi_e(x, mode, &result));
}

double gsl_sf_airy_Bi_scaled(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Bi_scaled_e(x, mode, &result));
}