#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <limits.h>
#include <math.h>
#include <complex.h>
#include <string.h>

#include "../numeric/SU21-numeric.h"
#include "../numeric/SU21-numeric-base.c"
#include "../numeric/SU21-fundamental-domain.c"

#define MAXNUMCANDIDATES 200
#define STACKSIZ 5000

// This program calculates the (hyperbolic) radius of the Dirichlet
// fundamental domain.

// Values associated to the radius
typedef struct {
  FLOAT_t R0;   // Radius of ball in $\CC^2$
  FLOAT_t HypDist;  // Hyperbolic distance to the origin
  FLOAT_t sVal;     // Equivalent Euclidean distance to the origin
  FLOAT_t HSSq;     // Squared Hilbert--Schmidt norm of a matrix~$g$
		    // which is unitary with respect to
		    // $\diag(1,1,-R0)$ and such that
		    // $\disthyp(g(0),0)=$ HypDist.
  // Here followthe same values for a point at twice that hyperbolic
  // distance to the origin.
  FLOAT_t DoubledHypDist;  // The double of HypDist
  FLOAT_t sValForDouble;  // The Euclidean distance associated to the
			  // double of HypDist
  FLOAT_t HSSqForDouble; // Squared Hilbert--Schmidt norm of a
			 // matrix~$g$ as above such that
			 // $\disthyp(g(0),0)=$ DoubledHypDist
} Dist_t;

// 3-simplex type
typedef struct {
  Pt_t Vertex[4]; // The four vertices, each of which should have
                  // (Euclidean) length~1
  Geom3Simp_t G3Simp; // As above, but expressed in terms of
                      // Vertex3 and the three difference vectors
  Pt_t MidPt; // Midpoint
  FLOAT_t s;  // s-value for the midpoint
  int PtIx; // Index of the point in Pts which fixes the s-value of
	    // the Midpoint
} ThreeSimp_t;

// Stack of 3-simplexes
typedef struct {
  ThreeSimp_t *First,*Ptr;
  int Siz;
} Stack_t;

void OutputSimplex(FILE *ofile,ThreeSimp_t Simp);

void Load16Simplexes(ThreeSimp_t **NewSimp_pp,Pt_t Pts[],int NumPts,
		     FLOAT_t *sMax_p);
void FinishSimp(ThreeSimp_t *Simp_p,Pt_t Pts[],int NumPts,FLOAT_t *sMax_p);
void ReduceSimplex(ThreeSimp_t *Simp_p,ThreeSimp_t **NewSimp_pp,
		   Pt_t Pts[],int NumPts,FLOAT_t *sMax_p);
void SplitSimplex(ThreeSimp_t *Simp_p,ThreeSimp_t **NewSimp_pp,
		  Pt_t Pts[],int NumPts,FLOAT_t *sMax_p);
int CompareSimps(const void *Ptr1,const void *Ptr2);

inline void PrePushN(Stack_t *Stack_p,const int N);
void InitializeStack(Stack_t *Stack_p);
void CheckRadius(FLOAT_t Radius,Pt_t Pts[],int NumPts);

void DistFromSVal(Dist_t *Dist_p);
void OutputDist(FILE *ofile,Dist_t Dist);

FLOAT_t MaxEccentricity;

int main(int argc,char *(argv[])) {

  // Input files
  FILE *EltsFile;
  // Output files
  FILE *FewEltsFile;

  char *Prefix_p,*NN_p,FileNam[MAX_FILENAME_LEN]="../";

  Elt_t *Elts;
  Pt_t *Pts;
  int NumElts,NumPts,PtIx,EltIx,NumFewElts,Iter,EqCnt;

  ThreeSimp_t *Candidates,*NewSimps,*NewSimp_p;
  int MaxNumCandidates=MAXNUMCANDIDATES,NumCandidates,CIx;
  Dist_t Dist;
  FLOAT_t sMax,sMaxSav,limit,Delta,Radius;

  // Get the prefix
  Prefix_p=getenv("PREFIX");
  assert(Prefix_p != NULL);

  // Open the files
  {
    size_t len=strlen(Prefix_p);
    assert(3+len+8<MAX_FILENAME_LEN);
    strcpy(&FileNam[3],Prefix_p);
    // Input files
    strcpy(&FileNam[3]+len,"-Elts");
    printf("%s\n",FileNam);
    EltsFile=fopen(FileNam,"r");
    assert(EltsFile != NULL);
    // Output files
    //strcpy(&FileNam[3]+len,"-FewElts");
    //printf("%s\n",FileNam);
    //FewEltsFile=fopen(FileNam,"w");
    //assert(FewEltsFile != NULL);
    fflush(stdout);
  }

  // Get the elements
  GetElts(EltsFile,"Elts",&NumElts,&Elts);
  // copy the points into the points vector
  Pts=malloc(NumElts*sizeof(Pt_t));
  for(PtIx=0;PtIx<NumElts;PtIx++)
    memcpy(&Pts[PtIx],&(Elts[PtIx].Pt),sizeof(Pt_t));
  NumPts=NumElts;

  //Radius=0.743731861776521-10*epsBig;
  //for(Delta=1.0;Delta>epsSmall;Delta/=10.0) {
  //  printf("\n===== Delta="FOCnv"\n",Delta);
  //  printf("===== Checking Radius+Delta="FOCnv"\n",Radius+Delta);
  //  CheckRadius(Radius+Delta,Pts,NumPts);
  //  printf("===== Checked Radius+Delta="FOCnv"\n\n",Radius+Delta);
  //}
  
  //MaxEccentricity=0.0;
  //printf("\n===== Checking Radius+epsSmall="FOCnv"\n",Radius+epsSmall);
  //CheckRadius(Radius+epsSmall,Pts,NumPts);
  //printf("===== Checked Radius+epsSmall="FOCnv"\n\n",Radius+epsSmall);
  //
  //printf("\n===== Checkinging Radius-epsSmall="FOCnv"\n",Radius-epsSmall);
  //CheckRadius(Radius-epsSmall,Pts,NumPts);
  //printf("===== Checked Radius-epsSmall="FOCnv"\n\n",Radius-epsSmall);
  //return;

  // Allocate space for 3-simplexes
  Candidates=malloc(MaxNumCandidates*sizeof(ThreeSimp_t));
  assert(Candidates != NULL);
  NewSimps=malloc(9*MaxNumCandidates*sizeof(ThreeSimp_t));
  assert(NewSimps != NULL);

  sMax=0.0; // Maximum s-value found so far
  MaxEccentricity=0.0; // Maximum eccentricity found so far

  // Load Candidates with 4-simplexes which cover all directions in the
  // sphere at infinity of $B(\CC^2)$
  NewSimp_p=Candidates;
  Load16Simplexes(&NewSimp_p,Pts,NumPts,&sMax);
  NumCandidates=16;

  // Find the largest possible s-Val
  sMaxSav=(-1.0);
  EqCnt=Iter=0;
  while(EqCnt<10) {
    if(sMaxSav == sMax)
      EqCnt++;
    else
      EqCnt=0;
    sMaxSav=sMax;
    printf("*** Iter=%4i NumCandidates=%i\n",Iter,NumCandidates);
    fflush(stdout);
    // Split each of the candidates into eight subsimplexes
    NewSimp_p=NewSimps;
    for(CIx=0;CIx<NumCandidates;CIx++) {
      ReduceSimplex(&(Candidates[CIx]),&NewSimp_p,Pts,NumPts,&sMax);
      SplitSimplex(&(Candidates[CIx]),&NewSimp_p,Pts,NumPts,&sMax);
    }
    // Sort the subsimplexes
    qsort(NewSimps,(size_t) (9*NumCandidates),sizeof(ThreeSimp_t),
	  CompareSimps);
    // The subsimplexes with the biggest s-values become the new
    // candidates
    NumCandidates=MIN(MaxNumCandidates,9*NumCandidates);
    memcpy(Candidates,NewSimps,NumCandidates*sizeof(ThreeSimp_t));
    Iter++;
  }
  free(Candidates);
  free(NewSimps);

  printf("\n===== Found sMax="FOCnv
	 "\n=====       MaxEccentricity="FOCnv"\n",
	 sMax,MaxEccentricity);

  printf("\n===== Checking sMax+epsBig="FOCnv"\n",sMax+epsBig);
  CheckRadius(sMax+epsBig,Pts,NumPts);
  printf("===== Checked sMax+epsBig="FOCnv"\n\n",sMax+epsBig);

  // Adjust it up just a little for safety
  sMax+=10*epsBig;

  // Set up the Dist structure
  Dist.R0=1.0;
  Dist.sVal=sMax;
  DistFromSVal(&Dist);
  // Display it
  printf("\n");
  OutputDist(stdout,Dist);

  // Determine how many elements in Elts shift the origin of
  // $B(\CC^2)$ no more than _twice_ the hyperbolic radius of the
  // Dirichlet fundamental domain.
  limit=Dist.sValForDouble*Dist.sValForDouble;
  for(EltIx=0;EltIx<NumElts;EltIx++)
    if(Elts[EltIx].d2 > limit)break;
  assert(EltIx<NumElts);
  assert(Elts[EltIx].d2 > limit+10*epsBig);
  NumFewElts=EltIx;
  printf("# of elements g such that d(g(0),0) <= twice the radius "
	 "of the fundamental domain = %i\n",NumFewElts);

  //// Produce FewEltsFile
  //fprintf(FewEltsFile,"NumFewElts=%i\n",NumFewElts);
  //for(EltIx=0;EltIx<NumFewElts;EltIx++)
  //  OutputElt(FewEltsFile,Elts[EltIx]);
}


// Into a vector-list of 3-simplexes, load sixteen simplexes
// covering all directions to infinity in~$B(\CC^2)$.

void Load16Simplexes(ThreeSimp_t **NewSimp_pp,Pt_t Pts[],int NumPts,
		     FLOAT_t *sMax_p) {
  int z1ReSgn,z1ImSgn,z2ReSgn,z2ImSgn,CIx;
  Geom3Simp_t *NewG3Simp_p;
  Pt_t V0,V1,V2,V3;

  for(z1ReSgn=1;z1ReSgn>=-1;z1ReSgn-=2)
    for(z1ImSgn=1;z1ImSgn>=-1;z1ImSgn-=2)
      for(z2ReSgn=1;z2ReSgn>=-1;z2ReSgn-=2)
	for(z2ImSgn=1;z2ImSgn>=-1;z2ImSgn-=2) {
	  V0[0] = (COMPLEX_t) z1ReSgn;
	  V0[1] = 0;
	  V1[0] = z1ImSgn*I;
	  V1[1] = 0;
	  V2[0] = 0;
	  V2[1] = (COMPLEX_t) z2ReSgn;
	  V3[0] = 0;
	  V3[1] = z2ImSgn*I;

	  memcpy((*NewSimp_pp)->Vertex[0],V0,sizeof(Pt_t));
	  memcpy((*NewSimp_pp)->Vertex[1],V1,sizeof(Pt_t));
	  memcpy((*NewSimp_pp)->Vertex[2],V2,sizeof(Pt_t));
	  memcpy((*NewSimp_pp)->Vertex[3],V3,sizeof(Pt_t));

	  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

	  memcpy(NewG3Simp_p->Vertex3,V3,sizeof(Pt_t));
	  SubPts(NewG3Simp_p->Diff[0],V0,V3);
	  SubPts(NewG3Simp_p->Diff[1],V1,V3);
	  SubPts(NewG3Simp_p->Diff[2],V2,V3);

	  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
	  (*NewSimp_pp)++;
	}
}

void OutputSimplex(FILE *ofile,ThreeSimp_t Simp) {
  int VIx;
  Pt_t Vertex;
  printf("+++++ SIMPLEX +++++\n");
  for(VIx=0;VIx<3;VIx++) {
    AddPts(Vertex, Simp.G3Simp.Vertex3, Simp.G3Simp.Diff[VIx]);
    OutputPt(ofile,Vertex);
  }
  OutputPt(ofile,Simp.G3Simp.Vertex3);
  fflush(stdout);
}


// Finish up a simplex which has just been added to the stack
void FinishSimp(ThreeSimp_t *Simp_p,Pt_t Pts[],int NumPts,FLOAT_t *sMax_p) {
  Pt_t Diff03Test,Diff13Test,Diff23Test;
  int Ix,VIx;
  FLOAT_t E;
  Geom3Simp_t *G3Simp_p;

  G3Simp_p=&(Simp_p->G3Simp);

  // Check for consistency
  SubPts(Diff03Test,Simp_p->Vertex[0],G3Simp_p->Vertex3);
  SubPts(Diff13Test,Simp_p->Vertex[1],G3Simp_p->Vertex3);
  SubPts(Diff23Test,Simp_p->Vertex[2],G3Simp_p->Vertex3);

  assert(SQRT(EucDist2(Simp_p->Vertex[3],G3Simp_p->Vertex3)) < epsSmall);
  assert(SQRT(EucDist2(Diff03Test,G3Simp_p->Diff[0])) < epsSmall);
  assert(SQRT(EucDist2(Diff13Test,G3Simp_p->Diff[1])) < epsSmall);
  assert(SQRT(EucDist2(Diff23Test,G3Simp_p->Diff[2])) < epsSmall);

  CheckGeom3Simp(*G3Simp_p);

  // Construct the midpoint of the simplex
  memcpy(Simp_p->MidPt,G3Simp_p->Vertex3,sizeof(Pt_t));
  for(VIx=0;VIx<3;VIx++)
    for(Ix=0;Ix<2;Ix++)
      (Simp_p->MidPt)[Ix]+= 0.25*(G3Simp_p->Diff)[VIx][Ix];

  // Normalize it to have Euclidean length 1
  NormalizePt(Simp_p->MidPt);
  // Calculate its s-value and the index of the point in Pts which
  // fixes that s-value:
  (Simp_p->s)=sVal(Simp_p->MidPt,Pts,NumPts,&(Simp_p->PtIx));
  // If appropriate, update *sMax_p
  if(Simp_p->s > *sMax_p) {
    *sMax_p = Simp_p->s;
    printf("sMax="FOCnv"\n",*sMax_p);
    OutputPt(stdout,Simp_p->MidPt);
    fflush(stdout);
  }
  // If appropriate, update MaxEccentricity
  E=Eccentricity(Simp_p->G3Simp);
  if(E>MaxEccentricity) MaxEccentricity = E;
}


// Make a copy of a three-simplex centered where the original was and
// somewhat smaller, and add it to a vector list of simplexes.
void ReduceSimplex(ThreeSimp_t *Simp_p,ThreeSimp_t **NewSimp_pp,
		  Pt_t Pts[],int NumPts,FLOAT_t *sMax_p) {
  const FLOAT_t Factor=0.75;
  Pt_t Displacement,Displacement3,MidPtDisp,MidPtDiff;
  Geom3Simp_t *OldG3Simp_p,*NewG3Simp_p;
  int VIx;

  for(VIx=0;VIx<4;VIx++) {
    SubPts(Displacement,(Simp_p->Vertex)[VIx],Simp_p->MidPt);
    Displacement[0]*=Factor;
    Displacement[1]*=Factor;
    AddPts(((*NewSimp_pp)->Vertex)[VIx],Displacement,Simp_p->MidPt);
    NormalizePt(((*NewSimp_pp)->Vertex)[VIx]);
  }
  
  OldG3Simp_p=&(Simp_p->G3Simp);
  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);
  MidPtDiff[0]=
    0.25*((OldG3Simp_p->Diff)[0][0]+
	  (OldG3Simp_p->Diff)[1][0]+
	  (OldG3Simp_p->Diff)[2][0]);
  MidPtDiff[1]=
    0.25*((OldG3Simp_p->Diff)[0][1]+
	  (OldG3Simp_p->Diff)[1][1]+
	  (OldG3Simp_p->Diff)[2][1]);
  NormalizeDiff(MidPtDiff,OldG3Simp_p->Vertex3);
  memcpy(Displacement3,MidPtDiff,sizeof(Pt_t));
  Displacement3[0]*=(1.0-Factor);
  Displacement3[1]*=(1.0-Factor);
  NormalizeDiff(Displacement3,OldG3Simp_p->Vertex3);
  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,Displacement3);
  
  for(VIx=0;VIx<3;VIx++) {
    SubPts(MidPtDisp,(OldG3Simp_p->Diff)[VIx],MidPtDiff);
    MidPtDisp[0]*=Factor;
    MidPtDisp[1]*=Factor;
    AddPts(Displacement,MidPtDisp,MidPtDiff);
    NormalizeDiff(Displacement,OldG3Simp_p->Vertex3);
    SubPts((NewG3Simp_p->Diff)[VIx],Displacement,Displacement3);
  }

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;
}


// Slit a three-simplex into eight smaller simplexes, and 
// put these onto a vector list of three-simplexes
void SplitSimplex(ThreeSimp_t *Simp_p,ThreeSimp_t **NewSimp_pp,
		  Pt_t Pts[],int NumPts,FLOAT_t *sMax_p) {
  Pt_t MidPt01,MidPt02,MidPt03,MidPt12,MidPt13,MidPt23;
  Pt_t DMidPt01,DMidPt02,DMidPt03,DMidPt12,DMidPt13,DMidPt23;
  FLOAT_t RealScal01,RealScal02,RealScal03;
  Geom3Simp_t *OldG3Simp_p,*NewG3Simp_p;

  // Construct the midpoints of the edges
  AddPts(MidPt01,Simp_p->Vertex[0],Simp_p->Vertex[1]);
  AddPts(MidPt02,Simp_p->Vertex[0],Simp_p->Vertex[2]);
  AddPts(MidPt03,Simp_p->Vertex[0],Simp_p->Vertex[3]);
  AddPts(MidPt12,Simp_p->Vertex[1],Simp_p->Vertex[2]);
  AddPts(MidPt13,Simp_p->Vertex[1],Simp_p->Vertex[3]);
  AddPts(MidPt23,Simp_p->Vertex[2],Simp_p->Vertex[3]);
  // Normalize them to have Euclidean length 1
  NormalizePt(MidPt01); NormalizePt(MidPt02);  NormalizePt(MidPt03); 
  NormalizePt(MidPt12); NormalizePt(MidPt13);  NormalizePt(MidPt23);

  OldG3Simp_p=&(Simp_p->G3Simp);

  // Construct the midpoints of the edges (in the form of difference
  // vectors)
  AddPts(DMidPt01,OldG3Simp_p->Diff[0],OldG3Simp_p->Diff[1]);
  AddPts(DMidPt02,OldG3Simp_p->Diff[0],OldG3Simp_p->Diff[2]);
  memcpy(DMidPt03,OldG3Simp_p->Diff[0],sizeof(Pt_t));
  AddPts(DMidPt12,OldG3Simp_p->Diff[1],OldG3Simp_p->Diff[2]);
  memcpy(DMidPt13,OldG3Simp_p->Diff[1],sizeof(Pt_t));
  memcpy(DMidPt23,OldG3Simp_p->Diff[2],sizeof(Pt_t));
  HalvePt(DMidPt01); HalvePt(DMidPt02); HalvePt(DMidPt03);
  HalvePt(DMidPt12); HalvePt(DMidPt13); HalvePt(DMidPt23);
  // Normalize them 
  NormalizeDiff(DMidPt01,OldG3Simp_p->Vertex3);
  NormalizeDiff(DMidPt02,OldG3Simp_p->Vertex3);
  NormalizeDiff(DMidPt03,OldG3Simp_p->Vertex3);
  NormalizeDiff(DMidPt12,OldG3Simp_p->Vertex3);
  NormalizeDiff(DMidPt13,OldG3Simp_p->Vertex3);
  NormalizeDiff(DMidPt23,OldG3Simp_p->Vertex3); 
  
  // Add the eight simplexes (tetrahedra) making up the original
  // simplex
  memcpy(((*NewSimp_pp)->Vertex)[0],Simp_p->Vertex[0],sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt01,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt02,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt03,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt03);
  SubPts((NewG3Simp_p->Diff)[0],OldG3Simp_p->Diff[0],DMidPt03);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt01,DMidPt03);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt02,DMidPt03);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;

  memcpy(((*NewSimp_pp)->Vertex)[0],Simp_p->Vertex[1],sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt01,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt12,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt13,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt13);
  SubPts((NewG3Simp_p->Diff)[0],OldG3Simp_p->Diff[1],DMidPt13);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt01,DMidPt13);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt12,DMidPt13);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;

  memcpy(((*NewSimp_pp)->Vertex)[0],Simp_p->Vertex[2],sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt23,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt12,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt02,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt02);
  SubPts((NewG3Simp_p->Diff)[0],OldG3Simp_p->Diff[2],DMidPt02);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt23,DMidPt02);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt12,DMidPt02);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;

  memcpy(((*NewSimp_pp)->Vertex)[0],Simp_p->Vertex[3],sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt23,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt13,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt03,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt03);
  NegPt((NewG3Simp_p->Diff)[0],DMidPt03);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt23,DMidPt03);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt13,DMidPt03);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;

  memcpy(((*NewSimp_pp)->Vertex)[0],MidPt01,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt23,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt02,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt12,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt12);
  SubPts((NewG3Simp_p->Diff)[0],DMidPt01,DMidPt12);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt23,DMidPt12);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt02,DMidPt12);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;
  
  memcpy(((*NewSimp_pp)->Vertex)[0],MidPt01,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt23,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt12,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt13,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt13);
  SubPts((NewG3Simp_p->Diff)[0],DMidPt01,DMidPt13);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt23,DMidPt13);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt12,DMidPt13);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;
  
  memcpy(((*NewSimp_pp)->Vertex)[0],MidPt01,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt23,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt13,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt03,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt03);
  SubPts((NewG3Simp_p->Diff)[0],DMidPt01,DMidPt03);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt23,DMidPt03);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt13,DMidPt03);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;
  
  memcpy(((*NewSimp_pp)->Vertex)[0],MidPt01,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[1],MidPt23,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[2],MidPt03,sizeof(Pt_t));
  memcpy(((*NewSimp_pp)->Vertex)[3],MidPt02,sizeof(Pt_t));

  NewG3Simp_p=&((*NewSimp_pp)->G3Simp);

  AddPts(NewG3Simp_p->Vertex3,OldG3Simp_p->Vertex3,DMidPt02);
  SubPts((NewG3Simp_p->Diff)[0],DMidPt01,DMidPt02);
  SubPts((NewG3Simp_p->Diff)[1],DMidPt23,DMidPt02);
  SubPts((NewG3Simp_p->Diff)[2],DMidPt03,DMidPt02);

  FinishSimp(*NewSimp_pp,Pts,NumPts,sMax_p);
  (*NewSimp_pp)++;
}

// Compare two simplexes, treating a simplex as _smaller_ if it has a
// _larger_ s-value.
int CompareSimps(const void *Ptr1,const void *Ptr2) {
  const ThreeSimp_t *Simp1=(const ThreeSimp_t *)Ptr1,
    *Simp2=(const ThreeSimp_t *)Ptr2;
  
  if(Simp1->s > Simp2->s)return -1;
  if(Simp1->s < Simp2->s)return +1;
  return 0;
}

void DistFromSVal(Dist_t *Dist_p) {
  FLOAT_t TmpSinh;
  const FLOAT_t R0=(Dist_p->R0);
  Dist_p->HypDist=ATANH(Dist_p->sVal / Dist_p->R0);
  Dist_p->DoubledHypDist=2.0*Dist_p->HypDist;
  Dist_p->sValForDouble=R0*TANH(Dist_p->DoubledHypDist);
  TmpSinh=SINH(Dist_p->HypDist);
  Dist_p->HSSq=3.0+(1.0/R0+R0)*(1.0/R0+R0)*TmpSinh*TmpSinh;
  TmpSinh=SINH(Dist_p->DoubledHypDist);
  Dist_p->HSSqForDouble=3.0+(1.0/R0+R0)*(1.0/R0+R0)*TmpSinh*TmpSinh;
}

void OutputDist(FILE *ofile,Dist_t Dist) {
  fprintf(ofile,
	  "                 R0 ="FOCnv
	  "                       R0^2 ="FOCnv"\n"
	  "  Hyperbolic Radius ="FOCnv
	  "             Doubled Radius ="FOCnv"\n"
	  " Radius (Euclidean) ="FOCnv
	  " Doubled radius (Euclidean) ="FOCnv"\n"
	  "        Euclidean^2 ="FOCnv
	  "     Euclidean^2 for Double ="FOCnv"\n"
	  "|.|_HS^2 for Radius ="FOCnv
	  "        |.|_HS^2 for Double ="FOCnv"\n\n",
	  Dist.R0,Dist.R0*Dist.R0,
	  Dist.HypDist,Dist.DoubledHypDist,
	  Dist.sVal,Dist.sValForDouble,
	  Dist.sVal*Dist.sVal,
	  Dist.sValForDouble*Dist.sValForDouble,
	  Dist.HSSq,Dist.HSSqForDouble);
}

// Initialize a stack of 3-simplexes
void InitializeStack(Stack_t *Stack_p) {
  (Stack_p->First)=(Stack_p->Ptr)
    =malloc(((size_t) Stack_p->Siz)*sizeof(ThreeSimp_t));
  assert(Stack_p->First != NULL);
}  

// Operations before pushing N elements onto a stack
inline void PrePushN(Stack_t *Stack_p,const int N) {
  assert(((Stack_p->Ptr)-(Stack_p->First))+N <= Stack_p->Siz);
}

// Checks that the Dirichlet fundamental domain is entirely within a
// given radius.

void CheckRadius(FLOAT_t Radius,Pt_t Pts[],int NumPts) {
  Stack_t Stack;
  FLOAT_t DummySMax,Dim,MinDim;
  ThreeSimp_t Simp;
  int NumChecked;
  

  // Set up stack
  Stack.Siz=STACKSIZ;
  InitializeStack(&Stack);

  // Load it with 16~simplexes that cover all directions in the sphere
  // at infinity for $\CC^2
  DummySMax=MaxEccentricity=0.0;
  NumChecked=0;
  MinDim=HUGE_FLOAT;
  PrePushN(&Stack,16);
  Load16Simplexes(&(Stack.Ptr),Pts,NumPts,&DummySMax);

  // Deal with the simplexes
  while(Stack.Ptr != Stack.First) {
    assert(DummySMax<=Radius);
    Stack.Ptr--;
    // Copy the simplex of interest off the stack
    memcpy(&Simp,Stack.Ptr,sizeof(ThreeSimp_t));
    NumChecked++;
    Dim=MaxEdgeLen(Simp.G3Simp);
    if(Dim<MinDim)MinDim=Dim;

    // Is there a possibility that this simplex contains a point with s-value
    // exceeding Radius?

    if(sValOnePt((Simp.Vertex)[0],Pts[Simp.PtIx]) > Radius ||
       sValOnePt((Simp.Vertex)[1],Pts[Simp.PtIx]) > Radius ||
       sValOnePt((Simp.Vertex)[2],Pts[Simp.PtIx]) > Radius ||
       sValOnePt((Simp.Vertex)[3],Pts[Simp.PtIx]) > Radius) {
      // If so, split it up and add the pieces to the stack.
      PrePushN(&Stack,8);
      SplitSimplex(&Simp,&(Stack.Ptr),Pts,NumPts,&DummySMax);
    }
  }
  printf("===== NumChecked=%12i\n===== MinDim="FOCnv" "
	 "\n===== DummySMax="FOCnv
	 "\n===== MaxEccentricity="FOCnv"\n",
	 NumChecked,MinDim,DummySMax,MaxEccentricity);
}