#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 {
  Geom3Simp_t G3Simp; // The four vertices, each of which should have
                      // (Euclidean) length~1, stored as Vertex3
                      // together with 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 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);
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 *CosetsFile,*KFile;
  // Output files
  FILE *FewCosetsFile;

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

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

  ThreeSimp_t *Candidates,*NewSimps,*NewSimp_p;
  Geom3Simp_t NewG3Simps[16];
  int MaxNumCandidates=MAXNUMCANDIDATES,NumCandidates,CIx,SimpIx;
  int KSiz,*KMult,*KInv;
  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+10<MAX_FILENAME_LEN);
    strcpy(&FileNam[3],Prefix_p);
    // Input files
    strcpy(&FileNam[3]+len,"-Cosets");
    printf("%s\n",FileNam);
    CosetsFile=fopen(FileNam,"r");
    assert(CosetsFile != NULL);
    strcpy(&FileNam[3]+len,"-K");
    printf("%s\n",FileNam);
    KFile=fopen(FileNam,"r");
    assert(KFile != NULL);
    // Output files
    strcpy(&FileNam[3]+len,"-FewCosets");
    printf("%s\n",FileNam);
    FewCosetsFile=fopen(FileNam,"w");
    assert(FewCosetsFile != NULL);
    fflush(stdout);
  }

  // Get the finite group K
  GetKEltsKK(KFile,&KSiz,&KElts,&KMult,&KInv);
  // Get the KK-cosets
  GetEltsString(CosetsFile,"NumKKCosets",&NumElts,&Elts);
  // copy the points into the points vector
  SetupPtsKK(&Pts,&NumPts,Elts,NumElts,KElts,KSiz);
  printf("NumKKCosets=%i NumPts=%i\n",NumElts,NumPts);
  fflush(stdout);

  //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)$
  Load16Geom3Simps(NewG3Simps);

  NewSimp_p=Candidates;
  for(SimpIx=0;SimpIx<16;SimpIx++) {
    memcpy(&(NewSimp_p->G3Simp),&(NewG3Simps[SimpIx]),
	   sizeof(Geom3Simp_t));
    FinishSimp(NewSimp_p,Pts,NumPts,&sMax);
    NewSimp_p++;
  }
  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);
    // For each of the candidate simplexes, generate 9~new simplexes
    // to try: a reduction of the original simplex, plus 8 subsimplexes
    // which fit together to make up the original.
    NewSimp_p=NewSimps;
    for(CIx=0;CIx<NumCandidates;CIx++) {
      ReduceGeom3Simp(&(NewSimp_p->G3Simp),Candidates[CIx].G3Simp);
      FinishSimp(NewSimp_p,Pts,NumPts,&sMax);
      NewSimp_p++;

      SplitGeom3Simp(NewG3Simps,Candidates[CIx].G3Simp);
      for(SimpIx=0;SimpIx<8;SimpIx++) {
	memcpy(&(NewSimp_p->G3Simp),&(NewG3Simps[SimpIx]),
	       sizeof(Geom3Simp_t));
	FinishSimp(NewSimp_p,Pts,NumPts,&sMax);
	NewSimp_p++;
      }
    }
    // 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);
  NumFewCosets=EltIx;
  printf("# of double cosets KgK such that d(g(0),0) <= twice the radius "
	 "of the fundamental domain = %i\n",NumFewCosets);

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

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 3-simplex data structure of which only the geometrical
// simplex is given
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);

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

// 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;
  Geom3Simp_t NewG3Simps[16];
  Pt_t V;
  int NumChecked,SimpIx;
  

  // 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;
  Load16Geom3Simps(NewG3Simps);
  for(SimpIx=0;SimpIx<16;SimpIx++) {
    assert(Stack.Ptr - Stack.First < Stack.Siz);
    memcpy(&(Stack.Ptr->G3Simp),&(NewG3Simps[SimpIx]),
	   sizeof(Geom3Simp_t));
    FinishSimp(Stack.Ptr,Pts,NumPts,&DummySMax);
    Stack.Ptr++;
  }

  // Deal with the simplexes on the stack
  MinDim=HUGE_FLOAT;
  NumChecked=0;

  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.G3Simp.Vertex3,Pts[Simp.PtIx]) > Radius ||

       (AddPts(V,Simp.G3Simp.Diff[0],Simp.G3Simp.Vertex3),
       sValOnePt(V,Pts[Simp.PtIx]) > Radius) ||

       (AddPts(V,Simp.G3Simp.Diff[1],Simp.G3Simp.Vertex3),
       sValOnePt(V,Pts[Simp.PtIx]) > Radius) ||

       (AddPts(V,Simp.G3Simp.Diff[2],Simp.G3Simp.Vertex3),
       sValOnePt(V,Pts[Simp.PtIx]) > Radius)) {

      // If so, split it up and add the pieces to the stack.
      SplitGeom3Simp(NewG3Simps,Simp.G3Simp);
      for(SimpIx=0;SimpIx<8;SimpIx++) {
	assert(Stack.Ptr - Stack.First < Stack.Siz);
	memcpy(&(Stack.Ptr->G3Simp),&(NewG3Simps[SimpIx]),
	       sizeof(Geom3Simp_t));
	FinishSimp(Stack.Ptr,Pts,NumPts,&DummySMax);
	Stack.Ptr++;
      }
    }
  }
  printf("===== NumChecked=%12i\n===== MinDim="FOCnv" "
	 "\n===== DummySMax="FOCnv
	 "\n===== MaxEccentricity="FOCnv"\n",
	 NumChecked,MinDim,DummySMax,MaxEccentricity);
}