#include "TCAD.h"
//#include "TMatrix.h"
ClassImp(TCAD)

 TCAD::TCAD(Char_t *grid, Char_t *pot,Int_t mode)
{
Int_t i=0;
Float_t point[3];
Float_t potential;

FILE *ing=fopen(grid,"r+"); 
if(ing==NULL) { printf("Error opening filen"); exit(0); }
FILE *inp=fopen(pot,"r+");
if(inp==NULL) { printf("Error opening filen"); exit(0); }

U=TArrayF (500000);    U.Reset();
Gx=TArrayF(500000);   Gx.Reset();
Gy=TArrayF(500000);   Gy.Reset();
Gz=TArrayF(500000);   Gz.Reset();
X=TArrayF (500000);    X.Reset();
Y=TArrayF (500000);    Y.Reset();
Z=TArrayF (500000);    Z.Reset();

while(!feof(ing))
{
  //  fscanf(ing,"%e %e %e",&point[0],&point[1],&point[2]);
  if(mode==0) {fscanf(ing,"%e %e",&point[0],&point[1]); point[2]=0;}
  else {fscanf(ing,"%e %e %e",&point[0],&point[1],&point[2]);}
  
  fscanf(inp,"%e",&potential);
  //  if(i%1000==0) printf("%e %e %e %en",point[0],point[1],point[2],potential);
  X[i]=point[0];  Y[i]=point[1];   Z[i]=point[2];   U[i]=potential;
  i++; 
} 
 num=i;

fclose(ing);
fclose(inp);
 SortAll();
 GetMaxMin();
}

 TCAD::~TCAD()
{
  //Clear();
}

 void TCAD::Print(Int_t start, Int_t end)
{
  for(Int_t i=start;i<end;i++)
    printf("point=%d, X=%f, Y=%f, Z=%f, U=%fn", i,X[i],Y[i],Z[i],U[i]);
}

 void TCAD::SortAll()
{
  Int_t k=0,q=0,fxi;
  Float_t fx;
  Sort2(0,num,"Z");
  Index[0][0]=0;
  for(Int_t i=0;i<=num;i++)
    {
      if( Z[i]!=Z[Index[k][0]] || i==num ) 
	{ 
	  k++; 
	  Index[k][0]=i; //IndexZ[k]=Z[i]; 
	  Sort2(Index[k-1][0],Index[k][0]-Index[k-1][0],"Y");
	  fx=Y[Index[k-1][0]]; fxi=Index[k-1][0];
	  //	  if(k==17) printf("Xselection:: fx=%f, fxi=%d n",fx,fxi);
	  for(Int_t j=Index[k-1][0];j<=Index[k][0];j++)
		 {
		   if(fx!=Y[j]) 
		     {
		       q++;
		       //		  if(k==17)  printf("Xselection:: fx=%f, fxi=%d n",fx,j-fxi);
		       Sort2(fxi,j-fxi,"X");
		       Index[k-1][q]=j;
		       fx=Y[j];
		       fxi=j;

		       
		     }
		     
		 }
	  q=0;
	  //  printf("%d %d %d %fn",k,Index[k][0],Index[k][0]-Index[k-1][0],Z[Index[k][0]]);
	}
      IndexNum1=k;
    } 
  q=0;

  //  printf("End of Arrayn");
}

 Int_t TCAD::FindPlane(Int_t dz,Float_t x,Float_t y,Int_t *Indexes)
{ 
Int_t i=0,j=0,foundy=0,foundx1=0,foundx2=0;
Int_t uy=1,dy=0,dx1=0,ux1=0,dx2=0,ux2=0,inc1=1,inc2=1;

while(!foundy)
	  {
	  if(Y[Index[dz][dy]]<=y && Y[Index[dz][uy]]>y) 
	    {
	      //	        printf("Y found (%f,%f)n",Y[Index[dz][dy]],Y[Index[dz][uy]]);
	      //while(!getchar());
	      if(foundx1==0) {dx1=Index[dz][dy]; ux1=Index[dz][dy]+inc1;}
	      while(!foundx1)
		{
		   if(X[dx1]<=x && X[ux1]>x)
		     {
		       //        printf("X1 found (%f,%f)n",X[dx1],X[ux1]);
		       // while(!getchar()); 
			 foundx1=1;
		     }
		   else {dx1++; ux1++;}
		   if(dx1>Index[dz][uy]) { dy--; break; }
		}
	      //	      printf("Searching for the second pointn");
	      if(foundx2==0) {dx2=Index[dz][uy]; ux2=Index[dz][uy]+inc2;}
              while(!foundx2)
		{
		   if(X[dx2]<=x && X[ux2]>x)
		     {
		       //printf("X2 found (%f,%f)n",X[dx2],X[ux2]);
		       foundx2=1; 
		       //        while(!getchar());
		     }
		   else {dx2++; ux2++;}
		   if(ux2>Index[dz][uy+1]) { uy++; break; }
		}

	      if(foundx1==1 && foundx2==1) 
		 if(X[dx1]!=X[dx2] && X[ux1]!=X[ux2])
		   if(ux1-dx1>ux2-dx2)
		     { inc2++; foundx2=0; /*printf("Switch2n");*/ }
		      else
		     { inc1++; foundx1=0; /*printf("Switch1n");*/ }
		 else
		   foundy=1;
	    }
	  else
	    {dy++; uy=dy+1;}	

	  if(dy<0) return(1); 
	  if(Index[dz][uy]==0) return(2); 
	  }

 Indexes[0]=dx1; Indexes[1]=ux1;  Indexes[2]=dx2; Indexes[3]=ux2; 
 //  printf("Index=%d (%f,%f,%f,U=%f)n",dx1,X[dx1],Y[dx1],Z[dx1],U[dx1]);
 // printf("Index=%d (%f,%f,%f,U=%f)n",ux1,X[ux1],Y[ux1],Z[ux1],U[ux1]);
 // printf("Index=%d (%f,%f,%f,U=%f)n",dx2,X[dx2],Y[dx2],Z[dx2],U[dx2]);
 // printf("Index=%d (%f,%f,%f,U=%f)n",ux2,X[ux2],Y[ux2],Z[ux2],U[ux2]);
 return(0);
}

 Float_t TCAD::FieldValue(Float_t x,Float_t y, Float_t z)
{
  Int_t i=0,j=0,k=0,foundz=0,foundy1=0,foundy2=0,foundx1=0,foundx2=0;
  Int_t dz=0,uz=1,uy1=0,dy1=0,uy2=0,dy2=0,dx1=0,ux1=0,dx2=0,ux2=0,inc1=1,inc2=1;
  Int_t Indexes[8],result1,result2;
  Float_t t,u,v;
  Float_t pot[8];
  Int_t maxiz,miniz,miniy,maxiy,minix,maxix;
  //  for(i=0;i<IndexNum1-1;i++) 
  if(x>MaxX || x<MinX || y>MaxY || y<MinY || z>MaxZ || z<MinZ ) { printf("Values out of scopen"); return(-1111);}
  while(!foundz)
    {
    if(Z[Index[dz][0]]<=z && (Z[Index[uz][0]]>z || Z[Index[uz][0]]==0)) 
      { 
	//printf("Z found (%f,%f)n",Z[Index[dz][0]],Z[Index[uz][0]]);
	result1=FindPlane(dz,x,y,Indexes);
	if(result1==1) dz--;
	if(result1==2) uz++;
	if(result1==0)  
	     {
	       if(Z[Index[dz][0]]==z) 
		 {
		   for(i=0;i<4;i++) Indexes[i+4]=Indexes[i];  
		 foundz=2;
		 }
		 else
		   {
	     result2=FindPlane(uz,x,y,&Indexes[4]);
             if(result2==1) dz--;
	     if(result2==1) uz++;
	     if(result2==0) foundz=1;
		   }
	     }
      }
    else
      {dz++; uz++;}
    
    


    }
 
  //  for(Int_t kk=0;kk<8;kk++) printf("%dn",Indexes[kk]);

  if(X[Indexes[1]]-X[Indexes[0]]==0) 
    t=1; else t=(x-X[Indexes[0]])/(X[Indexes[1]]-X[Indexes[0]]);
  if(Y[Indexes[2]]-Y[Indexes[1]]==0) 
    u=1; else u=(y-Y[Indexes[1]])/(Y[Indexes[2]]-Y[Indexes[1]]);
  if(Z[Indexes[4]]-Z[Indexes[0]]==0) 
    v=1; else v=(z-Z[Indexes[0]])/(Z[Indexes[4]]-Z[Indexes[0]]);
 if(foundz==2) v=0;
  // printf("t=%f u=%f v=%f n",t,u,v);
 


    return(  U[Indexes[0]]*(1-t)*(1-u)*(1-v)+  
  	     U[Indexes[1]]*t*(1-u)*(1-v)+
  	     U[Indexes[3]]*t*u*(1-v)+
  	     U[Indexes[2]]*(1-t)*u*(1-v)+
  	     U[Indexes[4]]*(1-t)*(1-u)*v+  
  	     U[Indexes[5]]*t*(1-u)*v+
  	     U[Indexes[7]]*t*u*v+
  	     U[Indexes[6]]*(1-t)*u*v   );
 
}



#define NRANSI
#define SWAP(a,b) temp=(a);(a)=(b);(b)=temp;
#define M 7
#define NSTACK 50

 void TCAD::Sort2(Int_t startnum, Int_t numele, Char_t *option)
{
        void nrerror(char error_text[]);
        void free_ivector(int *v, long nl, long nh);
        int *ivector(long nl, long nh);
	unsigned long i,ir=numele,j,k,l=1;
	int *istack,jstack=0;
	float a,b,c,d,temp;
	float *s,*s1,*s2,*s3;

	if(!strcmp(option,"Z")) {s=&(Z.GetArray())[startnum-1]; s1=&(X.GetArray())[startnum-1]; s2=&(Y.GetArray())[startnum-1]; s3=&(U.GetArray())[startnum-1];} 
	if(!strcmp(option,"X")) {s=&(X.GetArray())[startnum-1]; s1=&(Z.GetArray())[startnum-1]; s2=&(Y.GetArray())[startnum-1]; s3=&(U.GetArray())[startnum-1];}
	if(!strcmp(option,"Y")) {s=&(Y.GetArray())[startnum-1]; s1=&(Z.GetArray())[startnum-1]; s2=&(X.GetArray())[startnum-1]; s3=&(U.GetArray())[startnum-1];}

	  
	istack=ivector(1,NSTACK);
	for (;;) {
		if (ir-l < M) {
			for (j=l+1;j<=ir;j++) {
				a=s[j];
				b=s1[j];
				c=s2[j];
				d=s3[j];
				for (i=j-1;i>=1;i--) {
					if (s[i] <= a) break;
					s[i+1]=s[i];
					s1[i+1]=s1[i];
					s2[i+1]=s2[i];
					s3[i+1]=s3[i];
				}
				s[i+1]=a;
				s1[i+1]=b;
				s2[i+1]=c;
				s3[i+1]=d;
			}
			if (!jstack) {
				free_ivector(istack,1,NSTACK);
				return;
			}
			ir=istack[jstack];
			l=istack[jstack-1];
			jstack -= 2;
		} else {
			k=(l+ir) >> 1;
			SWAP(s[k],s[l+1])
			SWAP(s1[k],s1[l+1])
			SWAP(s2[k],s2[l+1])
			SWAP(s3[k],s3[l+1])

			if (s[l+1] > s[ir]) {
				SWAP(s[l+1],s[ir])
				SWAP(s1[l+1],s1[ir])
				SWAP(s2[l+1],s2[ir])
				SWAP(s3[l+1],s3[ir])
			}
			if (s[l] > s[ir]) {
				SWAP(s[l],s[ir])
				SWAP(s1[l],s1[ir])
				SWAP(s2[l],s2[ir])
				SWAP(s3[l],s3[ir])
			}
			if (s[l+1] > s[l]) {
				SWAP(s[l+1],s[l])
				SWAP(s1[l+1],s1[l])
				SWAP(s2[l+1],s2[l])
				SWAP(s3[l+1],s3[l])
			}
			i=l+1;
			j=ir;
			a=s[l];
			b=s1[l];
			c=s2[l];
			d=s3[l];
			for (;;) {
				do i++; while (s[i] < a);
				do j--; while (s[j] > a);
				if (j < i) break;
				SWAP(s[i],s[j])
				SWAP(s1[i],s1[j])
				SWAP(s2[i],s2[j])
				SWAP(s3[i],s3[j])
			}
			s[l]=s[j];
			s[j]=a;
			s1[l]=s1[j];
			s1[j]=b;
			s2[l]=s2[j];
			s2[j]=c;
			s3[l]=s3[j];
			s3[j]=d;


			jstack += 2;
			if (jstack > NSTACK) nrerror("NSTACK too small in sort2.");
			if (ir-i+1 >= j-l) {
				istack[jstack]=ir;
				istack[jstack-1]=i;
				ir=j-1;
			} else {
				istack[jstack]=j-1;
				istack[jstack-1]=l;
				l=i;
			}
		}
	}
}


#undef M
#undef NSTACK
#undef SWAP
#undef NRANSI


 void TCAD::GetMaxMin()
{
  Int_t i,j;
  MaxX=-1e30;
  MinX=1e30;
  MaxY=-1e30;;
  MinY=1e30;;
  MaxZ=-1e30;;
  MinZ=1e30;;
  
  for(i=0;i<num;i++)
    {
      if(X[i]>MaxX) MaxX=X[i];
      if(X[i]<MinX) MinX=X[i];
      if(Y[i]>MaxY) MaxY=Y[i];
      if(Y[i]<MinY) MinY=Y[i];
      if(Z[i]>MaxZ) MaxZ=Z[i];
      if(Z[i]<MinZ) MinZ=Z[i];
    }
}


 TH1F *TCAD::Slice1D(Option_t *axis, Axis_t where1,Axis_t where2)
{
 Float_t Point[3],field,up,down;
 Int_t which;

 
 if(!strcmp(axis,"z")) {Point[0]=(Float_t)where1; Point[1]=(Float_t)where2; which=2; up=MaxZ; down=MinZ;};
 if(!strcmp(axis,"x")) {Point[1]=(Float_t)where1; Point[2]=(Float_t)where2; which=0; up=MaxX; down=MinX; };
 if(!strcmp(axis,"y")) {Point[0]=(Float_t)where1; Point[2]=(Float_t)where2; which=1;  up=MaxY; down=MinY; };
 
 TH1F *h1=new TH1F("h1","h1",(Int_t)(up-down)+1,down,up);

 for(Int_t i=0;i<=(Int_t)(up-down);i++)
   {
         Point[which]=(Float_t)i;
	 if(i==(Int_t)(up-down)) Point[which]-=0.01;
	 field=FieldValue(Point[0],Point[1],Point[2]);
	 //	 	 printf("Point %d %fn",i,field);
	 h1->SetBinContent(i+1,field);	
   }
 return h1;
}

 TH2F *TCAD::Slice2D(Option_t *axis,Axis_t where)
{ Int_t p1,p2;
  Int_t Step=1; 
  Float_t up1,down1,up2,down2;
  Float_t Point[3];
  if(!strcmp(axis,"z")) {Point[2]=(Float_t) where; p1=0; up1=MaxX; down1=MinX; p2=1; up2=MaxY; down2=MinY; } 
  if(!strcmp(axis,"x")) {Point[0]=(Float_t) where; p1=1; up1=MaxY; down1=MinY; p2=2;  up2=MaxZ; down2=MinZ; } 
 if(!strcmp(axis,"y")) {Point[1]=(Float_t) where; p1=0; up1=MaxX; down1=MinX; p2=2; up2=MaxZ; down2=MinZ; } 

  TH2F *h2=new TH2F("h2","h2",(Int_t)(up1-down1)/Step+1,down1,up1,(Int_t)(up2-down2)/Step+1,down2,up2);
  printf("Dim1[%d]=%f-%f, Dim2[%d]=%f-%fn",(Int_t)(up1-down1)/Step+1,down1,up1,(Int_t)(up2-down2)/Step+1,down2,up2);
  for(Int_t i=0;i<=(up1-down1)/Step;i++) 
    {
     for(Int_t j=0;j<=(up2-down2)/Step;j++) 
       { 
	 Point[p1]=i*Step; Point[p2]=j*Step;
	 if(i==(Int_t)(up1-down1)) Point[p1]-=0.01;
	 if(j==(Int_t)(up2-down2)) Point[p2]-=0.01;
	
	 h2->SetBinContent(i+1,j+1, FieldValue(Point[0],Point[1],Point[2]));	
       }
    }
  return h2;
}