#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "cctk.h"
#include "cctk_Arguments.h"
#include "cctk_Parameters.h"

#include "CactusBase/Boundary/src/Boundary.h"
#include "CactusBase/CartGrid3D/src/Symmetry.h"
#include "CactusElliptic/EllBase/src/Ell_DBstructure.h"
 

void sor_flat_3d(cGH *GH, int FieldIndex, int MIndex, int NIndex,
		 CCTK_REAL *AbsTol, CCTK_REAL *RelTol)
{

  DECLARE_CCTK_PARAMETERS

  /* The pointer to the data fields */
  CCTK_REAL *Mlin=NULL, *Nlin=NULL;   
  CCTK_REAL *var =NULL; 

  /* shortcuts for deltas,etc. */
  CCTK_REAL dx,dy,dz; 

  /* Some physical variables */
  int accel_cheb=0, accel_const=0;
  int chebit;
  CCTK_REAL omega, resnorm, residual, glob_residual, rjacobian; 
  CCTK_REAL finf;
  CCTK_INT npow;
  CCTK_REAL tol;

  /* Iteration / stepping  variables */
  int sorit; 
  int i,is,ie;
  int j,js,je;
  int k,ks,ke,kstep;
  int nxyz;

  /* stencil index */
  int ijk;
  int ipjk, ijpk, ijkp, imjk, ijmk, ijkm;

  /* Coeeficients for the stencil...  */
  CCTK_REAL ac,ac_orig,aw,ae,an,as,at,ab;
 
  /* Miscellaneous */
  int sum_handle=-1;
  int sw[3], ierr;
  int Mstorage=0, Nstorage=0;
  static int firstcall = 1;
  CCTK_REAL  dx2rec, dy2rec, dz2rec; 
  

  /* Get the reduction handle */
  sum_handle = CCTK_ReductionArrayHandle("sum");
  if (sum_handle<0) 
    CCTK_WARN(1,"Cannot get reduction handle for operation >sum<");
  
  /* IF Robin BCs are set, prepare for a boundary call:
     setup stencil width and get Robin constants (set by the routine
     which is calling the solver interface) */
  if (CCTK_EQUALS(sor_bound,"robin")) { 
    sw[0]=1; 
    sw[1]=1; 
    sw[2]=1;
    
    ierr = Ell_GetRealKey(&finf, "EllLinConfMetric::Bnd::Robin::inf");
    ierr = Ell_GetIntKey (&npow, "EllLinConfMetric::Bnd::Robin::falloff");
  }

  /* Only supports absolute tolerance */
  tol   = AbsTol[0];
  if (CCTK_EQUALS(sor_accel,"const"))
    accel_const = 1;
  else if (CCTK_EQUALS(sor_accel,"cheb"))
    accel_cheb  = 1;

  /* Things to do only once! */
  if (firstcall==1) {
    if (CCTK_Equals(elliptic_verbose, "yes"))
      {
	if (accel_cheb)
	  printf("SOR with Chebyshev acceleration with radius of 1\n");
	else if (accel_const)
	  printf("SOR with hardcoded omega = 1.8\n");
	else
	  printf("SOR with unaccelearted relaxation (omega = 1)\n");
      }
    firstcall = 0;
  }

  /* Get the data ptr of these GFs, They all have to be
     on the same timelevel; if we have a negative index for M/N, 
     this GF is not set,  there for don't even look for it and flag it  */
  var = (CCTK_REAL*) CCTK_VarDataPtrI(GH, 0, FieldIndex);  
  if (MIndex>=0)  { 
    Mlin = (CCTK_REAL*) CCTK_VarDataPtrI(GH,0,MIndex);
    Mstorage = 1;
  }
  if (NIndex>=0) {
    Nlin = (CCTK_REAL*) CCTK_VarDataPtrI(GH,0,NIndex);
    Nstorage = 1;
  }

  /* Shortcuts */
  dx   = GH->cctk_delta_space[0];
  dy   = GH->cctk_delta_space[1];
  dz   = GH->cctk_delta_space[2];
  nxyz = GH->cctk_lsh[0]*GH->cctk_lsh[1]*GH->cctk_lsh[2];
  
  dx2rec = 1.0/(dx*dx);
  dy2rec = 1.0/(dy*dy);
  dz2rec = 1.0/(dz*dz);

  ae = dx2rec;
  aw = dx2rec;
  an = dy2rec;
  as = dy2rec;
  at = dz2rec;
  ab = dz2rec;

  ac_orig = -2.0*dx2rec - 2.0*dy2rec - 2.0*dz2rec;

  is = 1;
  js = 1;
  ie = GH->cctk_lsh[0]-1;
  je = GH->cctk_lsh[1]-1;
  ke = GH->cctk_lsh[2]-1;
  kstep = 2;

  /* start at 1 for historic (Fortran) reasons */
  for (sorit=1; sorit<=maxit; sorit++) {
    
    omega     =  1.0;
    rjacobian =  1.0;
    
    if (accel_cheb) 
      for (chebit=2;chebit<sorit;chebit++)
	omega = 1.0/(1.0 - 0.25*rjacobian*rjacobian*omega);
    if (accel_const)
      omega = 1.8;
    
    resnorm = 0.0;
    
    ks = (sorit%2)+1;
    if (GH->cctk_lsh[2]==3)
      ks = 2;
    
    for (k=ks;k<ke;k+=kstep) {
      for (j=js;j<je;j++)     {
	for (i=is;i<ie;i++)    {
	  
	  ac = ac_orig;

	  ijk   = CCTK_GFINDEX3D(GH,i  ,j  ,k  );
	  ipjk  = CCTK_GFINDEX3D(GH,i+1,j  ,k  );
	  imjk  = CCTK_GFINDEX3D(GH,i-1,j  ,k  );
	  ijpk  = CCTK_GFINDEX3D(GH,i  ,j+1,k  );
	  ijmk  = CCTK_GFINDEX3D(GH,i  ,j-1,k  );
	  ijkp  = CCTK_GFINDEX3D(GH,i  ,j  ,k+1);
	  ijkm  = CCTK_GFINDEX3D(GH,i  ,j  ,k-1);
	  
	  if (Mstorage)
	    ac += Mlin[ijk];

	  residual = ac * var[ijk]
	      + ae *var[ipjk]  +  aw*var[imjk]
	      + an *var[ijpk]  +  as*var[ijmk]
	      + at *var[ijkp]  +  ab*var[ijkm];

	  if (Nstorage)
	    residual +=Nlin[ijk];

	  resnorm  = resnorm + fabs(residual);

	  var[ijk] = var[ijk] - omega*residual/ac; 

	  printf(" %d %d %d  %f \n",i,j,k,var[ijk]);

	}
      }
    }

    /* reduction operation on processor-local residual values */
    ierr = CCTK_ReduceLocScalar(GH, -1, sum_handle,
				&resnorm, &glob_residual, CCTK_VARIABLE_REAL);
    if (ierr<0) 
      CCTK_WARN(1,"Reduction of Norm failed");

    glob_residual = glob_residual / 
      (GH->cctk_gsh[0]*GH->cctk_gsh[1]*GH->cctk_gsh[2]);

    /* apply symmetry boundary conditions within loop */    
    if (CartSymVI(GH,FieldIndex)<0) 
      CCTK_WARN(1,"CartSymVI failed in EllSOR loop");

    /* apply boundary conditions within loop */
    if (CCTK_EQUALS(sor_bound,"robin"))
      ierr = BndRobinVI(GH, sw, finf, npow,  FieldIndex);

    /* synchronization of grid variable */
    CCTK_SyncGroupWithVarI(GH, FieldIndex);

    /* Leave iteration loop if tolerance criterium is met */
    if (glob_residual<tol)
      break;
    
  }
  
  if (glob_residual>tol) 
    CCTK_WARN(2,"SOR SOLVER DID NOT CONVERGE");

  return;
}