/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/

#include <mpi.h>
#include <StGermain/StGermain.h>
#include <StgDomain/StgDomain.h>
#include <StgFEM/StgFEM.h>
#include <petsc.h>

#include "types.h"
#include "SemiLagrangianIntegrator.h"

#include <assert.h>

/** Textual name of this class */
const Type SemiLagrangianIntegrator_Type = "SemiLagrangianIntegrator";

SemiLagrangianIntegrator* _SemiLagrangianIntegrator_New(  SEMILAGRANGIANINTEGRATOR_DEFARGS  )
{
   SemiLagrangianIntegrator*		self;

   /* Allocate memory */
   assert( _sizeOfSelf >= sizeof(SemiLagrangianIntegrator) );
   /* The following terms are parameters that have been passed into this function but are being set before being passed onto the parent */
   /* This means that any values of these parameters that are passed into this function are not passed onto the parent function
      and so should be set to ZERO in any children of this class. */
   nameAllocationType = NON_GLOBAL;

   self = (SemiLagrangianIntegrator*) _Stg_Component_New(  STG_COMPONENT_PASSARGS  );

   /* General info */
   self->variableList = Stg_ObjectList_New();
   self->varStarList  = Stg_ObjectList_New();
   self->varOldList   = Stg_ObjectList_New();

   return self;
}

void* _SemiLagrangianIntegrator_Copy( void* slIntegrator, void* dest, Bool deep, Name nameExt, PtrMap* ptrMap ) {
   SemiLagrangianIntegrator*		self = (SemiLagrangianIntegrator*)slIntegrator;
   SemiLagrangianIntegrator*		newSemiLagrangianIntegrator;
   PtrMap*			map = ptrMap;
   Bool			ownMap = False;

   if( !map ) {
      map = PtrMap_New( 10 );
      ownMap = True;
   }

   newSemiLagrangianIntegrator = _Stg_Component_Copy( self, dest, deep, nameExt, map );

   if( deep ) {
      if( (newSemiLagrangianIntegrator->velocityField = PtrMap_Find( map, self->velocityField )) == NULL ) {
         newSemiLagrangianIntegrator->velocityField = Stg_Class_Copy( self->velocityField, NULL, deep, nameExt, map );
         PtrMap_Append( map, self->velocityField, newSemiLagrangianIntegrator->velocityField );
      }
   }
   else {
      newSemiLagrangianIntegrator->velocityField = Stg_Class_Copy( self->velocityField, NULL, deep, nameExt, map );
   }

   if( ownMap ) {
      Stg_Class_Delete( map );
   }

   return (void*)newSemiLagrangianIntegrator;
}

void _SemiLagrangianIntegrator_Delete( void* slIntegrator ) {
   SemiLagrangianIntegrator*		self = (SemiLagrangianIntegrator*)slIntegrator;
   Stg_Class_Delete( self->variableList );
   Stg_Class_Delete( self->varStarList );
   Stg_Class_Delete( self->varOldList );
}

void _SemiLagrangianIntegrator_Print( void* slIntegrator, Stream* stream ) {
   SemiLagrangianIntegrator*		self = (SemiLagrangianIntegrator*)slIntegrator;

   _Stg_Component_Print( self, stream );

   Journal_PrintPointer( stream, self->velocityField );
}

void* _SemiLagrangianIntegrator_DefaultNew( Name name ) {
   /* Variables set in this function */
   SizeT                                              _sizeOfSelf = sizeof(SemiLagrangianIntegrator);
   Type                                                      type = SemiLagrangianIntegrator_Type;
   Stg_Class_DeleteFunction*                              _delete = _SemiLagrangianIntegrator_Delete;
   Stg_Class_PrintFunction*                                _print = _SemiLagrangianIntegrator_Print;
   Stg_Class_CopyFunction*                                  _copy = _SemiLagrangianIntegrator_Copy;
   Stg_Component_DefaultConstructorFunction*  _defaultConstructor = _SemiLagrangianIntegrator_DefaultNew;
   Stg_Component_ConstructFunction*                    _construct = _SemiLagrangianIntegrator_AssignFromXML;
   Stg_Component_BuildFunction*                            _build = _SemiLagrangianIntegrator_Build;
   Stg_Component_InitialiseFunction*                  _initialise = _SemiLagrangianIntegrator_Initialise;
   Stg_Component_ExecuteFunction*                        _execute = _SemiLagrangianIntegrator_Execute;
   Stg_Component_DestroyFunction*                        _destroy = _SemiLagrangianIntegrator_Destroy;

   /* Variables that are set to ZERO are variables that will be set either by the current _New function or another parent _New function further up the hierachy */
   AllocationType  nameAllocationType = NON_GLOBAL /* default value NON_GLOBAL */;

   return (void*)_SemiLagrangianIntegrator_New(  SEMILAGRANGIANINTEGRATOR_PASSARGS  );
}

void _SemiLagrangianIntegrator_AssignFromXML( void* slIntegrator, Stg_ComponentFactory* cf, void* data ) {
   SemiLagrangianIntegrator*	self 		= (SemiLagrangianIntegrator*)slIntegrator;
   Dictionary*			dict;
   Dictionary_Entry_Value*		dev;
   unsigned			field_i;
   Name				fieldName;
   FeVariable*	   feVariable;

   Stg_Component_AssignFromXML( self, cf, data, False );

   self->context = Stg_ComponentFactory_ConstructByKey( cf, self->name, (Dictionary_Entry_Key)"Context", FiniteElementContext, False, data );
   if( !self->context  )
      self->context = Stg_ComponentFactory_ConstructByName( cf, (Name)"context", FiniteElementContext, True, data  );

   self->velocityField = Stg_ComponentFactory_ConstructByKey( cf, self->name, (Dictionary_Entry_Key)"VelocityField", FeVariable, False, NULL  );
   self->advectedField = Stg_ComponentFactory_ConstructByKey( cf, self->name, (Dictionary_Entry_Key)"AdvectedField", FeVariable, False, NULL  );

   dict = Dictionary_Entry_Value_AsDictionary( Dictionary_Get( cf->componentDict, (Dictionary_Entry_Key)self->name )  );
   dev  = Dictionary_Get( dict, (Dictionary_Entry_Key)"fields" );
   for( field_i = 0; field_i < Dictionary_Entry_Value_GetCount( dev ); field_i += 3  ) {
      fieldName = Dictionary_Entry_Value_AsString( Dictionary_Entry_Value_GetElement( dev, field_i ) );
      feVariable = Stg_ComponentFactory_ConstructByName( cf, (Name)fieldName, FeVariable, True, data  );
      Stg_ObjectList_Append( self->variableList, feVariable );

      /* the corresponding _* field */
      fieldName = Dictionary_Entry_Value_AsString( Dictionary_Entry_Value_GetElement( dev, field_i + 1 ) );
      feVariable = Stg_ComponentFactory_ConstructByName( cf, (Name)fieldName, FeVariable, True, data  );
      Stg_ObjectList_Append( self->varStarList, feVariable );

      /* the corresponding old field */
      fieldName = Dictionary_Entry_Value_AsString( Dictionary_Entry_Value_GetElement( dev, field_i + 2 ) );
      feVariable = Stg_ComponentFactory_ConstructByName( cf, (Name)fieldName, FeVariable, True, data  );
      Stg_ObjectList_Append( self->varOldList, feVariable );
   }

//   self->sle = Stg_ComponentFactory_ConstructByKey( cf, self->name, (Dictionary_Entry_Key)"SLE", Energy_SLE, False, NULL  );

   /* for problems with temporally evolving velocity */
   self->prevVelField = Stg_ComponentFactory_ConstructByKey( cf, self->name, (Dictionary_Entry_Key)"PreviousTimeStepVelocityField", FeVariable, False, data );
   if( self->prevVelField  ) {
      EP_AppendClassHook( Context_GetEntryPoint( self->context, AbstractContext_EP_UpdateClass ), SemiLagrangianIntegrator_UpdatePreviousVelocityField, self );
      EP_InsertClassHookAfter( Context_GetEntryPoint( self->context, AbstractContext_EP_UpdateClass ), "SemiLagrangianIntegrator_UpdatePreviousVelocityField", SemiLagrangianIntegrator_InitSolve, self );
   } else {
      EP_AppendClassHook( Context_GetEntryPoint( self->context, AbstractContext_EP_UpdateClass ), SemiLagrangianIntegrator_InitSolve, self );
   }

//   if( self->sle ) {
//      /** also set sle to run where required */
//      EP_InsertClassHookAfter( Context_GetEntryPoint( self->context, AbstractContext_EP_UpdateClass ), "SemiLagrangianIntegrator_InitSolve", SystemLinearEquations_GetRunEPFunction(), self->sle );
//      /** remember to disable the standard run at execute */
//      SystemLinearEquations_SetRunDuringExecutePhase( self->sle, False);
//   }

   self->isConstructed = True;
}

void _SemiLagrangianIntegrator_Build( void* slIntegrator, void* data ) {
   SemiLagrangianIntegrator*	self 		= (SemiLagrangianIntegrator*)slIntegrator;
   FeVariable*			feVariable;
   FeVariable*			feVarOld;
   FeVariable*			feVarStar;
   unsigned			   field_i;

   if(self->velocityField) Stg_Component_Build(self->velocityField, data, False);
   if(self->advectedField) Stg_Component_Build(self->advectedField, data, False);
   if(self->prevVelField)  Stg_Component_Build(self->prevVelField , data, False);

   for( field_i = 0; field_i < self->variableList->count; field_i++ ) {
      feVariable = (FeVariable*) self->variableList->data[field_i];
      feVarOld   = (FeVariable*) self->varOldList->data[field_i];
      feVarStar  = (FeVariable*) self->varStarList->data[field_i];
      if(feVariable) Stg_Component_Build(feVariable, data, False);
      if(feVarOld  ) Stg_Component_Build(feVarOld  , data, False);
      if(feVarStar ) Stg_Component_Build(feVarStar , data, False);
   }
}

void _SemiLagrangianIntegrator_Initialise( void* slIntegrator, void* data ) {
   SemiLagrangianIntegrator*	self 		= (SemiLagrangianIntegrator*)slIntegrator;
   FeVariable*			feVariable;
   FeVariable*			feVarOld;
   FeVariable*			feVarStar;
   unsigned			   field_i;

   if(self->velocityField) Stg_Component_Initialise(self->velocityField, data, False);
   if(self->advectedField) Stg_Component_Initialise(self->advectedField, data, False);
   if(self->prevVelField) {
      Stg_Component_Initialise(self->prevVelField , data, False);
      SemiLagrangianIntegrator_UpdatePreviousVelocityField( slIntegrator, NULL );
   }

   for( field_i = 0; field_i < self->variableList->count; field_i++ ) {
      feVariable = (FeVariable*) self->variableList->data[field_i];
      feVarOld   = (FeVariable*) self->varOldList->data[field_i];
      feVarStar  = (FeVariable*) self->varStarList->data[field_i];
      if(feVariable) Stg_Component_Initialise(feVariable, data, False);
      if(feVarOld  ) Stg_Component_Initialise(feVarOld  , data, False);
      if(feVarStar ) Stg_Component_Initialise(feVarStar , data, False);
   }
}

void _SemiLagrangianIntegrator_Execute( void* slIntegrator, void* data ) {
}

void _SemiLagrangianIntegrator_Destroy( void* slIntegrator, void* data ) {
   SemiLagrangianIntegrator*	self 	= (SemiLagrangianIntegrator*)slIntegrator;
   FeVariable*			feVariable;
   FeVariable*			feVarOld;
   FeVariable*			feVarStar;
   unsigned			   field_i;

   if(self->velocityField) Stg_Component_Destroy(self->velocityField, data, False);
   if(self->advectedField) Stg_Component_Destroy(self->advectedField, data, False);
   if(self->prevVelField)  Stg_Component_Destroy(self->prevVelField , data, False);

   for( field_i = 0; field_i < self->variableList->count; field_i++ ) {
      feVariable = (FeVariable*) self->variableList->data[field_i];
      feVarOld   = (FeVariable*) self->varOldList->data[field_i];
      feVarStar  = (FeVariable*) self->varStarList->data[field_i];
      if(feVariable) Stg_Component_Destroy(feVariable, data, False);
      if(feVarOld  ) Stg_Component_Destroy(feVarOld  , data, False);
      if(feVarStar ) Stg_Component_Destroy(feVarStar , data, False);
   }
}

void SemiLagrangianIntegrator_InitSolve( void* _self, void* _context ) {
   SemiLagrangianIntegrator*	self			= (SemiLagrangianIntegrator*) _self;
   unsigned			   field_i, node_i;
   FeVariable*			feVariable;
   FeVariable*			feVarOld;
   FeVariable*			feVarStar;
   double            phi[3];
   unsigned			   lMeshSize;
   FeMesh*				mesh;

   for( field_i = 0; field_i < self->variableList->count; field_i++ ) {
      feVariable = (FeVariable*) self->variableList->data[field_i];
      feVarStar  = (FeVariable*) self->varStarList->data[field_i];
      feVarOld   = (FeVariable*) self->varOldList->data[field_i];

      /* we're assuming that the solution vector has already been updated onto the FeVariable (in the SLE class) */
      mesh = feVariable->feMesh;
      lMeshSize = Mesh_GetLocalSize( mesh, MT_VERTEX );
      for( node_i = 0; node_i < lMeshSize; node_i++ ) {
         FeVariable_GetValueAtNode( feVariable, node_i, phi );
         FeVariable_SetValueAtNode( feVarOld, node_i, phi );
      }
      FeVariable_SyncShadowValues( feVarOld );

      /* generate the _* field */
      SemiLagrangianIntegrator_Solve( self, feVarOld, feVarStar );
   }
}

#define INV6 0.166666666667

void IntegrateRungeKutta( FeVariable* velocityField, double dt, double* origin, double* position ) {
   unsigned		ndims		     = Mesh_GetDimSize( velocityField->feMesh );
   unsigned		dim_i;
   double			min[3], max[3];
   double			k[4][3];
   double			coordPrime[3];
   unsigned*		periodic	     = ((CartesianGenerator*)velocityField->feMesh->generator)->periodic;

   Mesh_GetGlobalCoordRange( velocityField->feMesh, min, max );

   FieldVariable_InterpolateValueAt( velocityField, origin, k[0] );
   for( dim_i = 0; dim_i < ndims; dim_i++ ) {
      coordPrime[dim_i] = origin[dim_i] - 0.5 * dt * k[0][dim_i];
      PeriodicUpdate( coordPrime, min, max, dim_i, periodic[dim_i] );
   }
   FieldVariable_InterpolateValueAt( velocityField, coordPrime, k[1] );

   for( dim_i = 0; dim_i < ndims; dim_i++ ) {
      coordPrime[dim_i] = origin[dim_i] - 0.5 * dt * k[1][dim_i];
      PeriodicUpdate( coordPrime, min, max, dim_i, periodic[dim_i] );
   }
   FieldVariable_InterpolateValueAt( velocityField, coordPrime, k[2] );

   for( dim_i = 0; dim_i < ndims; dim_i++ ) {
      coordPrime[dim_i] = origin[dim_i] - dt * k[2][dim_i];
      PeriodicUpdate( coordPrime, min, max, dim_i, periodic[dim_i] );
   }
   FieldVariable_InterpolateValueAt( velocityField, coordPrime, k[3] );

   for( dim_i = 0; dim_i < ndims; dim_i++ ) {
      position[dim_i] = origin[dim_i] -
         INV6 * dt * ( k[0][dim_i] + 2.0 * k[1][dim_i] + 2.0 * k[2][dim_i] + k[3][dim_i] );
      PeriodicUpdate( position, min, max, dim_i, periodic[dim_i] );
   }
}

/* 2nd order acurate runge kutta algorithm for interpolating backwards in time through a temporally evolving velocity field
   Durran, D. "Numerical Methods for Wave Equations in Geophysical Fluid Dynamics" (1999), pages 310-313

   u(t^(n+0.5)) = 1.5u(t^n) - 0.5u(t^(n-1))
   x_* = x^(n+1) - 0.5*dt*u(x^(n+1),t^n)
   x_j^n = x^(n+1) - dt*u(x_*,t^(n+0.5))

   Force term:
      F = 0.5*[3*S(x_j^n,t^n) - S(x_j^(n-1),t^(n-1))]
 */
void IntegrateRungeKutta_StgVariableVelocity( FeVariable* currVelField, FeVariable* interVelField, double dt, double* origin, double* position ) {
   unsigned		nDims		     = Mesh_GetDimSize( currVelField->feMesh );
   unsigned		dim_i;
   double			min[3], max[3];
   double			midPoint[3];
   double			velCurr[3], velInter[3];
   CartesianGenerator*	gen		     = (CartesianGenerator*)currVelField->feMesh->generator;

   Mesh_GetGlobalCoordRange( currVelField->feMesh, min, max );

   FieldVariable_InterpolateValueAt( currVelField, origin, velCurr );
   for( dim_i = 0; dim_i < nDims; dim_i++ ) {
      midPoint[dim_i] = origin[dim_i] - 0.5 * dt * velCurr[dim_i];
      PeriodicUpdate( midPoint, min, max, dim_i, gen->periodic[dim_i] );
   }

   FieldVariable_InterpolateValueAt( interVelField, midPoint, velInter );

   /* 2nd order approximation of velocity at time current + dt/2 */
   for( dim_i = 0; dim_i < nDims; dim_i++ ) {
      position[dim_i] = origin[dim_i] - dt * velInter[dim_i];
      PeriodicUpdate( position, min, max, dim_i, gen->periodic[dim_i] );
   }
}

/* for case of temporally evolving velocity field, when we need to integrate backwards in time
 * throught this to find our take off point */
void SemiLagrangianIntegrator_UpdatePreviousVelocityField( void* _self, void* _context ) {
   SemiLagrangianIntegrator*	self		= (SemiLagrangianIntegrator*) _self;
   FeVariable*			currVelField	= self->velocityField;
   FeVariable*			prevVelField	= self->prevVelField;
   unsigned			node_i;
   double				vel[3];

   for( node_i = 0; node_i < Mesh_GetLocalSize( currVelField->feMesh, MT_VERTEX ); node_i++ ) {
      FeVariable_GetValueAtNode( currVelField, node_i, vel );
      FeVariable_SetValueAtNode( prevVelField, node_i, vel );
   }

   FeVariable_SyncShadowValues( currVelField );
   FeVariable_SyncShadowValues( prevVelField );
}

/* cubic Lagrangian interpoation in 1-D */
void InterpLagrange( double x, double* coords, double (*values)[3], unsigned numdofs, double* result ) {
   unsigned	node_i, dof_i;
   unsigned	otherIndices[3];
   unsigned	otherIndexCount, otherIndex_i;
   double		factor;

   for( dof_i = 0; dof_i < numdofs; dof_i++ )
      result[dof_i] = 0.0;

   for( node_i = 0; node_i < 4; node_i++ ) {
      otherIndexCount = 0;
      for( otherIndex_i = 0; otherIndex_i < 4; otherIndex_i++ )
         if( otherIndex_i != node_i )
            otherIndices[otherIndexCount++] = otherIndex_i;

      factor = 1.0;
      for( otherIndex_i = 0; otherIndex_i < 3; otherIndex_i++ )
         factor *= ( x - coords[otherIndices[otherIndex_i]] ) / ( coords[node_i] - coords[otherIndices[otherIndex_i]] );

      for( dof_i = 0; dof_i < numdofs; dof_i++ ) {
         result[dof_i] += ( values[node_i][dof_i] * factor );
      }
   }
}

Bool PeriodicUpdate( double* pos, double* min, double* max, unsigned dim, Bool isPeriodic ) {
  if( pos[dim] < min[dim] ) {
    pos[dim] = (isPeriodic) ? max[dim] - min[dim] + pos[dim] : min[dim];
    return True;
  }
  if( pos[dim] > max[dim] ) {
    pos[dim] = (isPeriodic) ? min[dim] - max[dim] + pos[dim] : max[dim];
    return True;
  }

  return False;
}

Bool BicubicInterpolatorNew( FeVariable* feVariable, FeVariable* stencilField, double* position, unsigned* sizes, double* result ) {
  /* Calculated the BicubicInterpolation of the feVariable at position
   *
   * Input Args:
   *   feVariable: the field to be interpolated at `position`.
   *   stencilField: the field of initial spline stencils for each node.
   *   position:   the position of interpolatation.
   *   sizes:      memory chunk of size = sizeof(double)*dim.
   *   results:    the interpolated value.
   *
   *
   * Returns Values:
   *  True: if interpolation successful
   *  False: position is not in 'domain' of local processor
   */

   FeMesh*	feMesh = feVariable->feMesh;
   unsigned	nInc ;
   int    ijk[3];
   int		x_i, y_i, z_i, *inc;
   double localMin[3], localMax[3], double_ijk[3];
   Index  gNode_I, lNode_I, elementIndex;
   double px[4], py[4], pz[4];
   unsigned	nodeIndex[4][4];
   unsigned	node_I3D[4][4][4];
   unsigned	nDims   = Mesh_GetDimSize( feMesh );
   unsigned	numdofs = feVariable->dofLayout->dofCounts[0];
   double   ptsX[4][3], ptsY[4][3], ptsZ[4][3];
   Bool     inDomain  = True;

   inDomain = Mesh_SearchElements( feMesh, position, &elementIndex ); // get the element id

   if (!inDomain) return False;

   FeMesh_GetElementNodes( feMesh, elementIndex, feVariable->inc ); // get the incidence graph (inc.) of nodes on the element
   nInc = IArray_GetSize( feVariable->inc );                        // from inc. get the number of nodes on the element
   inc = IArray_GetPtr( feVariable->inc );                          // get the node ids from inc.

   FeVariable_GetValueAtNode( stencilField, inc[0], &(double_ijk[0]) );
   ijk[0] = lround(double_ijk[0]);
   ijk[1] = lround(double_ijk[1]);
   ijk[2] = lround(double_ijk[2]);

   /* interpolate using Lagrange's formula */
   if( nDims == 2 ) {
      for( y_i = 0; y_i < 4; y_i++ )
         for( x_i = 0; x_i < 4; x_i++ ) {
            gNode_I = ijk[0] + x_i + ( ijk[1] + y_i ) * sizes[0];
            if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, gNode_I, &lNode_I ) ){
              printf("Error in %s, trying to build an interpolation to position (%g, %g) using node %d, a non domain node, in interpolator.\n", __func__, position[0], position[1], gNode_I);
              abort();
            }
            else
               nodeIndex[x_i][y_i] = lNode_I;
         }
   }
   else {
      for( z_i = 0; z_i < 4; z_i++ )
         for( y_i = 0; y_i < 4; y_i++ )
            for( x_i = 0; x_i < 4; x_i++ ) {
               gNode_I = ijk[0] + x_i + ( ijk[1] + y_i ) * sizes[0] + ( ijk[2] + z_i ) * sizes[0] * sizes[1];
               if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, gNode_I, &lNode_I ) ) {
                 printf("Error in %s, trying to build an interpolation to position (%g, %g, %g) using node %d, a non domain node, in interpolator.\n", __func__, position[0], position[1], position[2], gNode_I);
                 abort();
               }
               else
                  node_I3D[x_i][y_i][z_i] = lNode_I;
            }
   }

   if( nDims == 3 ) {
      for( x_i = 0; x_i < 4; x_i++ )
         px[x_i] = Mesh_GetVertex( feMesh, node_I3D[x_i][0][0] )[0];
      for( y_i = 0; y_i < 4; y_i++ )
         py[y_i] = Mesh_GetVertex( feMesh, node_I3D[0][y_i][0] )[1];
      for( z_i = 0; z_i < 4; z_i++ )
         pz[z_i] = Mesh_GetVertex( feMesh, node_I3D[0][0][z_i] )[2];

      for( z_i = 0; z_i < 4; z_i++ ) {
         for( y_i = 0; y_i < 4; y_i++ ) {
            for( x_i = 0; x_i < 4; x_i++ )
               FeVariable_GetValueAtNode( feVariable, node_I3D[x_i][y_i][z_i], ptsX[x_i] );

            InterpLagrange( position[0], px, ptsX, numdofs, ptsY[y_i] );
         }

         InterpLagrange( position[1], py, ptsY, numdofs, ptsZ[z_i] );
      }

      InterpLagrange( position[2], pz, ptsZ, numdofs, result );
   }
   else {
      for( x_i = 0; x_i < 4; x_i++ )
         px[x_i] = Mesh_GetVertex( feMesh, nodeIndex[x_i][0] )[0];
      for( y_i = 0; y_i < 4; y_i++ )
         py[y_i] = Mesh_GetVertex( feMesh, nodeIndex[0][y_i] )[1];

      for( y_i = 0; y_i < 4; y_i++ ) {
         for( x_i = 0; x_i < 4; x_i++ )
            FeVariable_GetValueAtNode( feVariable, nodeIndex[x_i][y_i], ptsX[x_i] );

         InterpLagrange( position[0], px, ptsX, numdofs, ptsY[y_i] );
      }

      InterpLagrange( position[1], py, ptsY, numdofs, result );
   }

   return True;
}

Bool SemiLagrangianIntegrator_PointsAreClose( double* p1, double* p2, int dim, double rtol, double atol ) {
  /* check if two points are within rtol (relative tolerance)
   * or atol (absolute tolerance)
   *
   * Intput Parameters:
   *  p1   : point 1
   *  p2   : point 2
   *  dim  : the dimensions of points
   *  rtol : the relative tolerance
   *  atol : the absolute tolerance
   *
   * Return Values:
   *  True:  if points are close
   *  False: if points are not close
   */

  double disp[3], p2_norm, length;

  StGermain_VectorSubtraction(disp, p1, p2, dim); // displacement vector
  length  = StGermain_VectorMagnitude(disp, dim); // length of vector
  p2_norm = StGermain_VectorMagnitude(p2, dim);   // original size of p2

  if (length < rtol*p2_norm + atol) return True;

  return False;
}

void SemiLagrangianIntegrator_BuildStaticStencils( FeVariable* stencilField ) {
  /* Function to build the node indices for cubic interpolation.
   * The idea is to find a record the starting node indices (ijk) for each interpolant stencil.
   *
   * **NOTE**: We never want to Sync the stencilField FeVariable shadow values, ie.
   *  FeVariable_SyncShadowValues( stencilField ), because the field contains processor
   *  specific ordering and Syncing with shadow space will produce erroroneous starting indicies
   *
   */

  FeMesh* feMesh = stencilField->feMesh;
  Grid **grid;
  int d_i,ijk[3];
  double double_ijk[3];
  unsigned *sizes,try,nDims,n_i, nNodes;
  Index gNode_I;

  nDims = Mesh_GetDimSize( feMesh );
  nNodes = Mesh_GetDomainSize( feMesh, MT_VERTEX );
  grid = (Grid**)ExtensionManager_Get( feMesh->info, feMesh, feMesh->vertGridId  );
  sizes = Grid_GetSizes(*grid);

  for( n_i=0; n_i<nNodes; n_i++) {
    /* For every domain node, build the stencil starting location, ijk */
    gNode_I = Mesh_DomainToGlobal( feMesh, MT_VERTEX, n_i );
    Grid_Lift( *grid, gNode_I, ijk );

    /* for every dim try go one node back */
    for(d_i=0; d_i<nDims; d_i++) {
      if( ijk[d_i] == 0 ) { continue; } // skip if at 0
      ijk[d_i]--;
      try = Grid_Project( *grid, ijk );
      // if not in domain space don't go back one
      if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, try, &try ) ) {
        ijk[d_i]++;
      }
    }

    /* for every dim try go 3 forward */
    for(d_i=0; d_i<nDims; d_i++) {
      ijk[d_i]+=3;

      // if we go off the edge, go back one
      if( ijk[d_i] >= sizes[d_i] ) {
        ijk[d_i] -= 4;
        continue;
      }
      try = Grid_Project( *grid, ijk );
      // if not in domain space go back one, else reset
      if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, try, &try ) ) {
        ijk[d_i]-=4;
      } else {
        ijk[d_i]-=3;
      }
    }

    double_ijk[0] = (double)ijk[0];
    double_ijk[1] = (double)ijk[1];
    double_ijk[2] = (double)ijk[2];

    // save ijk as the starting point
    FeVariable_SetValueAtNode( stencilField, n_i, &(double_ijk[0]) );
   }
}

void SemiLagrangianIntegrator_SolveNew( FeVariable* variableField, double dt, FeVariable* velocityField, FeVariable* varStarField, FeVariable* stencilField ) {
  /* Function evaluates varStarField - an interpolation of variableField taken at the departure points.
   * Departure points are positions taken from the nodes and advected backwards along the characteristic curves.
   * The interpolation method used is a cubic spline and the implementation is only compatible with orthogonal meshes.
   *
   * Input Args:
   *   feVariable:    the original field to be interpolated.
   *   dt:            the time step size to go backwards along the characteristic, it should NOT be > CFL condition.
   *   velocityField: the velocity field used to go backward.
   *   stencilField:  the field of initial spline stencils for each node.
   *   varStarField:  the resultant field of interpolated variableField taken at the departure points.
   *
   */

   FeMesh*   feMesh   = variableField->feMesh;
   unsigned  meshSize = Mesh_GetLocalSize( feMesh, MT_VERTEX );
   unsigned  nDims    = Mesh_GetDimSize( feMesh );
   Grid**    nodegrid = (Grid**) Mesh_GetExtension( feMesh, Grid*,  feMesh->vertGridId );
   unsigned* sizes    = Grid_GetSizes( *nodegrid );

   unsigned  node_I;
   double    var[3], x_i[3], delta[3], minLength, *x_0;
   Bool      result;

   Mesh_GetMinimumSeparation( feMesh, &minLength, delta );

   /* sync parallel field variables to get shadow values */
   FeVariable_SyncShadowValues( velocityField );
   FeVariable_SyncShadowValues( variableField );

   /* assume that the variable mesh is the same as the velocity mesh */
   for( node_I = 0; node_I < meshSize; node_I++ ) {
      /* find the position back in time (u*), x_i */
      x_0 = Mesh_GetVertex(feMesh, node_I);
      IntegrateRungeKutta( velocityField, dt, x_0, x_i );

      /* if the new x_i is "close" to original node, don't Bicubuic Interpolate, take original node value */
      if( SemiLagrangianIntegrator_PointsAreClose(x_i, x_0, nDims, 0, 1e-6*minLength) ) {
        FeVariable_GetValueAtNode( variableField, node_I, var );
      } else {
        result = BicubicInterpolatorNew(variableField, stencilField, x_i, sizes, var);

        /* if BicubicInerpolator returns false, x_i was not found in domain space.
         * Fallback to using the node value. */
        if(result == False) { FeVariable_GetValueAtNode( variableField, node_I, var ); }
      }

      FeVariable_SetValueAtNode( varStarField, node_I, var );
   }

   /* sync interpolated values */
   FeVariable_SyncShadowValues( varStarField );
}

Bool BicubicInterpolator( FeVariable* feVariable, double* position, double* delta, unsigned* nNodes, double* result ) {
  /* Calculated the BicubicInterpolation of the feVariable at position
   *
   * Input Args:
   *   feVariable: the field to be interpolated at `position`.
   *   position:   the position of interpolatation.
   *   delta:      memory chunk of size = sizeof(double)*dim.
   *   nNodes:     numbers of nodes in ijk representation.
   *   results:    the interpolated value.
   *
   *
   * Returns Values:
   *  True: if interpolation successful
   *  False: position is not in 'domain' of local processor
   */

   FeMesh*	feMesh			= feVariable->feMesh;
   unsigned	nInc ;
   int    ijk[3];
   int		x_i, y_i, z_i, *inc;
   double localMin[3], localMax[3];
   Index  gNode_I, lNode_I, elementIndex;
   double px[4], py[4], pz[4];
   unsigned	nodeIndex[4][4];
   unsigned	node_I3D[4][4][4];
   unsigned	nDims   = Mesh_GetDimSize( feMesh );
   unsigned	numdofs = feVariable->dofLayout->dofCounts[0];
   double   ptsX[4][3], ptsY[4][3], ptsZ[4][3];
   Bool     inDomain  = True;

   inDomain = Mesh_SearchElements( feMesh, position, &elementIndex ); // get the element id

   if (!inDomain) return False;

   FeMesh_GetElementNodes( feMesh, elementIndex, feVariable->inc ); // get the incidence graph (inc.) of nodes on the element
   nInc = IArray_GetSize( feVariable->inc );                        // from inc. get the number of nodes on the element
   inc = IArray_GetPtr( feVariable->inc );                          // get the node ids from inc.

   if( nInc % 3 == 0 ) /* quadratic elements */ {
      delta[0] = Mesh_GetVertex( feMesh, inc[1] )[0] - Mesh_GetVertex( feMesh, inc[0] )[0];
      delta[1] = Mesh_GetVertex( feMesh, inc[3] )[1] - Mesh_GetVertex( feMesh, inc[0] )[1];
      if( nDims == 3 )
         delta[2] = Mesh_GetVertex( feMesh, inc[9] )[2] - Mesh_GetVertex( feMesh, inc[0] )[2];
   }
   else {
      delta[0] = Mesh_GetVertex( feMesh, inc[1] )[0] - Mesh_GetVertex( feMesh, inc[0] )[0];
      delta[1] = Mesh_GetVertex( feMesh, inc[2] )[1] - Mesh_GetVertex( feMesh, inc[0] )[1];
      if( nDims == 3 )
         delta[2] = Mesh_GetVertex( feMesh, inc[4] )[2] - Mesh_GetVertex( feMesh, inc[0] )[2];
   }

   Mesh_GetLocalCoordRange( feMesh, localMin, localMax );
   gNode_I = Mesh_DomainToGlobal( feMesh, MT_VERTEX, inc[0] );

   {
    Grid **grid;
    int d_i;
    unsigned *sizes;
    grid = (Grid**)ExtensionManager_Get( feMesh->info, feMesh, feMesh->vertGridId  );
    Grid_Lift( *grid, gNode_I, ijk );
    sizes = Grid_GetSizes(*grid);

    if( nInc % 2 == 0 ) {
      unsigned try;
      /* for every dim try go one node back */
      for(d_i=0; d_i<nDims; d_i++) {
        if( ijk[d_i] == 0 ) { continue; } // skip if at 0
        ijk[d_i]--;
        try = Grid_Project( *grid, ijk );
        // if not in domain space don't go back one
        if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, try, &try ) ) {
          ijk[d_i]++;
        }
      }
      /* for every dim try go 3 forward */
      for(d_i=0; d_i<nDims; d_i++) {
        ijk[d_i]+=3;

        // if we go off the edge, go back one
        if( ijk[d_i] >= sizes[d_i] ) {
          ijk[d_i] -= 4;
          continue;
        }
        try = Grid_Project( *grid, ijk );
        // if not in domain space go back one, else reset
        if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, try, &try ) ) {
          ijk[d_i]-=4;
        } else {
          ijk[d_i]-=3;
        }
      }
    }
   }

   /* interpolate using Lagrange's formula */
   if( nDims == 2 ) {
      for( y_i = 0; y_i < 4; y_i++ )
         for( x_i = 0; x_i < 4; x_i++ ) {
            gNode_I = ijk[0] + x_i + ( ijk[1] + y_i ) * nNodes[0];
            if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, gNode_I, &lNode_I ) ){
              printf("Error in BicubicInterpolator(), trying to build an interpolation to position (%g, %g) using node %d, a non domain node, in interpolator.\n", position[0], position[1], gNode_I);
              abort();
            }
            else
               nodeIndex[x_i][y_i] = lNode_I;
         }
   }
   else {
      for( z_i = 0; z_i < 4; z_i++ )
         for( y_i = 0; y_i < 4; y_i++ )
            for( x_i = 0; x_i < 4; x_i++ ) {
               gNode_I = ijk[0] + x_i + ( ijk[1] + y_i ) * nNodes[0] + ( ijk[2] + z_i ) * nNodes[0] * nNodes[1];
               if( !Mesh_GlobalToDomain( feMesh, MT_VERTEX, gNode_I, &lNode_I ) ) {
                 printf("Error in BicubicInterpolator(), trying to build an interpolation to position (%g, %g, %g) using node %d, a non domain node, in interpolator.\n", position[0], position[1], position[2], gNode_I);
                 abort();
               }
               else
                  node_I3D[x_i][y_i][z_i] = lNode_I;
            }
   }

   if( nDims == 3 ) {
      for( x_i = 0; x_i < 4; x_i++ )
         px[x_i] = Mesh_GetVertex( feMesh, node_I3D[x_i][0][0] )[0];
      for( y_i = 0; y_i < 4; y_i++ )
         py[y_i] = Mesh_GetVertex( feMesh, node_I3D[0][y_i][0] )[1];
      for( z_i = 0; z_i < 4; z_i++ )
         pz[z_i] = Mesh_GetVertex( feMesh, node_I3D[0][0][z_i] )[2];

      for( z_i = 0; z_i < 4; z_i++ ) {
         for( y_i = 0; y_i < 4; y_i++ ) {
            for( x_i = 0; x_i < 4; x_i++ )
               FeVariable_GetValueAtNode( feVariable, node_I3D[x_i][y_i][z_i], ptsX[x_i] );

            InterpLagrange( position[0], px, ptsX, numdofs, ptsY[y_i] );
         }

         InterpLagrange( position[1], py, ptsY, numdofs, ptsZ[z_i] );
      }

      InterpLagrange( position[2], pz, ptsZ, numdofs, result );
   }
   else {
      for( x_i = 0; x_i < 4; x_i++ )
         px[x_i] = Mesh_GetVertex( feMesh, nodeIndex[x_i][0] )[0];
      for( y_i = 0; y_i < 4; y_i++ )
         py[y_i] = Mesh_GetVertex( feMesh, nodeIndex[0][y_i] )[1];

      for( y_i = 0; y_i < 4; y_i++ ) {
         for( x_i = 0; x_i < 4; x_i++ )
            FeVariable_GetValueAtNode( feVariable, nodeIndex[x_i][y_i], ptsX[x_i] );

         InterpLagrange( position[0], px, ptsX, numdofs, ptsY[y_i] );
      }

      InterpLagrange( position[1], py, ptsY, numdofs, result );
   }

   return True;
}

void SemiLagrangianIntegrator_Solve( void* slIntegrator, FeVariable* variableField, FeVariable* varStarField ) {
   SemiLagrangianIntegrator*	self 		     = (SemiLagrangianIntegrator*)slIntegrator;
   FiniteElementContext*		context		     = self->context;
   unsigned			node_I;
   FeMesh*			feMesh		     = variableField->feMesh;
   unsigned			meshSize	     = Mesh_GetLocalSize( feMesh, MT_VERTEX );
   FeVariable*			velocityField	     = self->velocityField;
   double				dt		     = AbstractContext_Dt( context );
   unsigned			dim_I;
   unsigned			nDims		     = Mesh_GetDimSize( feMesh );
   double				position[3];
   double				var[3];
   Grid**				grid		     = (Grid**) Mesh_GetExtension( feMesh, Grid*,  feMesh->elGridId );
   unsigned*    sizes		     = Grid_GetSizes( *grid );
   unsigned			nNodes[3];
   double				delta[3];
   unsigned			nInc;
   int*         inc;

   FeMesh_GetElementNodes( variableField->feMesh, 0, variableField->inc );
   nInc = IArray_GetSize( variableField->inc );
   inc = IArray_GetPtr( variableField->inc );

   delta[0] = Mesh_GetVertex( feMesh, inc[1] )[0] - Mesh_GetVertex( feMesh, inc[0] )[0];
   if( nInc % 3 == 0 ) /* quadratic elements */ {
      delta[1] = Mesh_GetVertex( feMesh, inc[3] )[1] - Mesh_GetVertex( feMesh, inc[0] )[1];
      if( nDims == 3 )
         delta[2] = Mesh_GetVertex( feMesh, inc[9] )[2] - Mesh_GetVertex( feMesh, inc[0] )[2];
      for( dim_I = 0; dim_I < nDims; dim_I++ )
         nNodes[dim_I] = 2 * sizes[dim_I] + 1;
   }
   else {
      delta[1] = Mesh_GetVertex( feMesh, inc[2] )[1] - Mesh_GetVertex( feMesh, inc[0] )[1];
      if( nDims == 3 )
         delta[2] = Mesh_GetVertex( feMesh, inc[4] )[2] - Mesh_GetVertex( feMesh, inc[0] )[2];

      for( dim_I = 0; dim_I < nDims; dim_I++ )
         nNodes[dim_I] = sizes[dim_I] + 1;
   }

   FeVariable_SyncShadowValues( velocityField );
   FeVariable_SyncShadowValues( variableField );

   /* assume that the variable mesh is the same as the velocity mesh */
   for( node_I = 0; node_I < meshSize; node_I++ ) {
      /* find the position back in time (u*) */
      IntegrateRungeKutta( velocityField, dt, Mesh_GetVertex(feMesh, node_I), position );

      /* create a bicubic interpolation of variableField at u* */
      BicubicInterpolator( variableField, position, delta, nNodes, var );

      FeVariable_SetValueAtNode( varStarField, node_I, var );
   }
   FeVariable_SyncShadowValues( varStarField );
}


/************************************************************************
 Tuple.h.h - Copyright marcello

 Here you can write a license for your code, some comments or any other
 information you want to have in your generated code. To to this simply
 configure the "headings" directory in uml to point to a directory
 where you have your heading files.

 or you can just replace the contents of this file with your own.
 If you want to do this, this file is located at

 /usr/share/apps/umbrello/headings/heading.h

 -->Code Generators searches for heading files based on the file extension
 i.e. it will look for a file name ending in ".h" to include in C++ header
 files, and for a file name ending in ".java" to include in all generated
 java code.
 If you name the file "heading.<extension>", Code Generator will always
 choose this file even if there are other files with the same extension in the
 directory. If you name the file something else, it must be the only one with that
 extension in the directory to guarantee that Code Generator will choose it.

 you can use variables in your heading files which are replaced at generation
 time. possible variables are : author, date, time, filename and filepath.
 just write %variable_name%

 This file was generated on Sat Nov 10 2007 at 15:05:38
 The original location of this file is /home/marcello/Projects/fitted/Developing/Tuple.h
 **************************************************************************/

#ifndef TUPLE_H
#define TUPLE_H

#include <fstream>
#include <vector>
//#include <gsl/gsl_vector.h>

//<< Sam
typedef enum
{
  L1,
  L2,
  Linfty
} NormType;

using namespace std;

namespace PoliFitted
{

/**
 * Tuples are sequence containers representing arrays with additional functionalities.
 *
 * Just like arrays, tuples use contiguous storage locations for their elements, which
 * means that their elements can also be accessed using offsets on regular pointers to its elements,
 * and just as efficiently as in arrays.
 *
 * Like arrays, internally, tuples do not store information about the dimension that must be handled
 * manually from outside the class.
 */
class Tuple
{
  public:

    /**
     * Converts N vectors to a single tuple obtained by
     * concatenation of the vectors (double) in the given order
     * Vectors are received as sequence of const_iterator:
     * v1.begin, v1.end, v2.begin, v2.end, v3.begin ...
     *
     * @param count    final number of elements of the tuple.
     Sum of size of each vector
     * @param ...      sequence of const_iterator (begin,end,begin,end,...)
     * @return the reference of the new tuple
     */
    //static Tuple* CONVERT(unsigned int count, ...);

    // Constructors/Destructors
    //

    /**
     * Empty Constructor
     */
    Tuple();

    /**
     * Construct a tuple of specified size
     * @param size the size of the tuple
     */
    Tuple(unsigned int size);

    /**
     * Empty Destructor
     */
    ~Tuple();

    /**
     * Returns a reference to the element at position \f$pos\f$ in the tuple.
     *
     * Programs should never call this function with an argument \f$pos\f$ that is
     * out of range, since this causes undefined behavior.
     * @brief Access element
     * @param pos Position of an element in the container. Notice that the first element has a position of 0 (not 1).
     * @return The element at the specified position in the tuple.
     */
    float& operator[](const unsigned int pos);

    /**
     * Returns a reference to the element at position \f$pos\f$ in the tuple.
     *
     * The function does not automatically check whether \f$pos\f$ is within the
     * bounds of valid elements in the vector.
     * Programs should never call this function with an argument \f$pos\f$ that is
     * out of range, since this causes undefined behavior.
     *
     * @brief Access element
     * @param pos Position of an element in the container. Notice that the first element has a position of 0 (not 1).
     * @return The element at the specified position in the container.
     */
    float& at(const unsigned int pos);

    /**
     * Compute a distance between the current and a given tuple. In other words,
     * it computes the p-norm between tuples.
     * \a p must be one of: "L1", "L2", "Linfty". "L1" is the absolute distance,
     * "L2" is the Euclidean distance, and "Linfty" is the maximum norm.
     * The norm is computed taking into account the first \a size elements of
     * the current and given tuples.
     *
     * Programs should never call this function with an argument \f$size\f$ that is
     * out of range for one of the two tuples, since this causes undefined behavior.
     *
     * @brief Distance computation
     * @param t An other tuple.
     * @param size the number of elements to be considered.
     * @param type the norm type
     * @return the \a p-norm
     */
    float Distance(Tuple* t, unsigned int size, NormType type = L2);

    /**
     * Assigns new contents to the tuple, replacing its current contents, without modifying its size.
     *
     * Since the dimension of the tuple is not changed accordingly to the given array,
     * the behavior is undefined when \a size is greater than the current tuple dimension.
     * In this case it is necessary to call the complementary function Tuple::Resize.
     *
     * The tuple is filled starting from the first elements.
     *
     * @brief Assign elements
     * @param values Array storing the new elements.
     * @param size The size of the given array
     */
    void SetValues(float* values, unsigned int size);

    /**
     * Returns a direct pointer to the memory array used internally by the tuple to store its owned elements.
     *
     * Because elements in the tuple are guaranteed to be stored in contiguous storage locations in the same
     * order as represented by the tuple, the pointer retrieved can be offset to access any element in the array.
     * @brief Access data
     * @return A pointer to the first element in the array used internally by the tuple.
     */
    float* GetValues();

    /**
     * Returns a clone of the tuple by copying the first \a input_size elements.
     *
     * @brief Clone object
     * @param input_size the number of elements to be copied
     * @return A pointer to the new tuple.
     */
    Tuple* Clone(unsigned int input_size);

    /**
     * Resizes the container so that it contains \a new_size elements.
     *
     * If \a new_size is smaller than the current container size, the content is reduced to its first n elements,
     * removing those beyond (and destroying them).
     * If \a new_size is greater than the current container size, the content is expanded by inserting at the end
     * as many elements as needed to reach a size of \a new_size.
     * The value of the newly allocated portion is indeterminate.
     *
     * Notice that this function changes the actual content of the container by inserting or erasing elements
     * from it.
     *
     * @brief Change size
     * @param new_size New container size, expressed in number of elements.
     */
    void Resize(unsigned int new_size);

    template<class T>
    T* GetValues(unsigned int input_size);

    //gsl_vector* GetGSLVector(unsigned int input_size);

    std::vector<double> ToVector(unsigned int input_size);

  protected:

  private:

    float* mValues;

};

inline float& Tuple::at(const unsigned int pos)
{
  return mValues[pos];
}

inline float& Tuple::operator[](const unsigned int pos)
{
  return mValues[pos];
}

inline void Tuple::SetValues(float* values, unsigned int size)
{
  for (unsigned int i = 0; i < size; i++)
  {
    mValues[i] = values[i];
  }
}

inline float* Tuple::GetValues()
{
  return mValues;
}

template<class T>
inline T* Tuple::GetValues(unsigned int input_size)
{
  T* values = new T[input_size];
  for (unsigned int i = 0; i < input_size; i++)
  {
    values[i] = (T) mValues[i];
  }
  return values;
}

/*inline gsl_vector* Tuple::GetGSLVector(unsigned int input_size)
 {
 gsl_vector* values = gsl_vector_alloc(input_size);
 for (unsigned int i = 0; i < input_size; i++)
 {
 gsl_vector_set(values, i, mValues[i]);
 }
 return values;
 }*/

inline std::vector<double> Tuple::ToVector(unsigned int input_size)
{
  std::vector<double> values;
  for (unsigned int i = 0; i < input_size; i++)
  {
    values.push_back(mValues[i]);
  }
  return values;
}

}

#endif // TUPLE_H


/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 */

#pragma once
#ifndef COMMON_H
#define COMMON_H

// Since we call cblas_dgemm in openmp for loop,
// we call "extension" APIs for setting the number of threads.
#ifdef USE_INTEL_MKL
#include <mkl.h>
	#if INTEL_MKL_VERSION < 20170000
		// Will throw an error at development time in non-standard settings
		PLEASE DONOT COMPILE SHARED LIBRARIES WITH OLDER MKL VERSIONS
	#endif
#include <mkl_service.h>
extern "C" void mkl_set_num_threads(int numThreads);
#else
#include <cblas.h>
extern "C" void openblas_set_num_threads(int numThreads);
#endif

template<class FP>
size_t computeNNZ(FP* arr, int limit) {
    size_t nnz = 0;
#ifndef USE_INTEL_MKL
#pragma omp parallel for reduction(+: nnz)
#endif
    for(int i=0; i<limit; i++)
        nnz += (arr[i]!=0) ? 1 : 0;
    return nnz;
}

static int SYSDS_CURRENT_NUM_THREADS = -1;
static void setNumThreadsForBLAS(int numThreads) {
	if (SYSDS_CURRENT_NUM_THREADS != numThreads) {
#ifdef USE_OPEN_BLAS
		openblas_set_num_threads(numThreads);
#else
		mkl_set_num_threads(numThreads);
#endif
		SYSDS_CURRENT_NUM_THREADS = numThreads;
	}
}

#endif // COMMON_H


#pragma once

#include "Cesium3DTiles/Library.h"
#include "CesiumGltf/Model.h"
#include <functional>
#include <glm/mat4x4.hpp>
#include <gsl/span>
#include <optional>

namespace Cesium3DTiles {

/**
 * @brief Functions for loading and processing glTF models.
 *
 * This class offers basic functions for loading glTF models as
 * `CesiumGltf::Model` instances, and for processing the mesh primitives that
 * appear in the resulting models.
 */
class CESIUM3DTILES_API Gltf final {

public:
  /** @brief This class cannot be instantiated */
  Gltf() = delete;

  /**
   * @brief A callback function for {@link Gltf::forEachPrimitiveInScene}.
   */
  typedef void ForEachPrimitiveInSceneCallback(
      CesiumGltf::Model& gltf,
      CesiumGltf::Node& node,
      CesiumGltf::Mesh& mesh,
      CesiumGltf::MeshPrimitive& primitive,
      const glm::dmat4& transform);

  /**
   * @brief Apply the given callback to all relevant primitives.
   *
   * If the given `sceneID` is non-negative and exists in the given glTF,
   * then the given callback will be applied to all meshes of this scene.
   *
   * If the given `sceneId` is negative, then the meshes that the callback
   * will be applied to depends on the structure of the glTF model:
   *
   * * If the glTF model has a default scene, then it will
   *   be applied to all meshes of the default scene.
   * * Otherwise, it will be applied to all meshes of the the first scene.
   * * Otherwise (if the glTF model does not contain any scenes), it will
   *   be applied to all meshes that can be found by starting a traversal
   *   at the root node.
   * * Otherwise (if there are no scenes and no nodes), then all meshes
   *   will be traversed.
   *
   * @param gltf The glTF model.
   * @param sceneID The scene ID (index)
   * @param callback The callback to apply
   */
  static void forEachPrimitiveInScene(
      CesiumGltf::Model& gltf,
      int sceneID,
      std::function<ForEachPrimitiveInSceneCallback>&& callback);

  /**
   * @brief A callback function for {@link Gltf::forEachPrimitiveInScene}.
   */
  typedef void ForEachPrimitiveInSceneConstCallback(
      const CesiumGltf::Model& gltf,
      const CesiumGltf::Node& node,
      const CesiumGltf::Mesh& mesh,
      const CesiumGltf::MeshPrimitive& primitive,
      const glm::dmat4& transform);

  /** @copydoc Gltf::forEachPrimitiveInScene() */
  static void forEachPrimitiveInScene(
      const CesiumGltf::Model& gltf,
      int sceneID,
      std::function<ForEachPrimitiveInSceneConstCallback>&& callback);
};

} // namespace Cesium3DTiles


/* vector/gsl_vector_long_double.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, Brian Gough
 *
 * 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.
 */

#ifndef __GSL_VECTOR_LONG_DOUBLE_H__
#define __GSL_VECTOR_LONG_DOUBLE_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_inline.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_block_long_double.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size;
  size_t stride;
  long double *data;
  gsl_block_long_double *block;
  int owner;
}
gsl_vector_long_double;

typedef struct
{
  gsl_vector_long_double vector;
} _gsl_vector_long_double_view;

typedef _gsl_vector_long_double_view gsl_vector_long_double_view;

typedef struct
{
  gsl_vector_long_double vector;
} _gsl_vector_long_double_const_view;

typedef const _gsl_vector_long_double_const_view gsl_vector_long_double_const_view;

/* Allocation */

GSL_FUN gsl_vector_long_double *gsl_vector_long_double_alloc (const size_t n);
GSL_FUN gsl_vector_long_double *gsl_vector_long_double_calloc (const size_t n);

GSL_FUN gsl_vector_long_double *gsl_vector_long_double_alloc_from_block (gsl_block_long_double * b,
                                                     const size_t offset,
                                                     const size_t n,
                                                     const size_t stride);

GSL_FUN gsl_vector_long_double *gsl_vector_long_double_alloc_from_vector (gsl_vector_long_double * v,
                                                      const size_t offset,
                                                      const size_t n,
                                                      const size_t stride);

GSL_FUN void gsl_vector_long_double_free (gsl_vector_long_double * v);

/* Views */

GSL_FUN _gsl_vector_long_double_view
gsl_vector_long_double_view_array (long double *v, size_t n);

GSL_FUN _gsl_vector_long_double_view
gsl_vector_long_double_view_array_with_stride (long double *base,
                                         size_t stride,
                                         size_t n);

GSL_FUN _gsl_vector_long_double_const_view
gsl_vector_long_double_const_view_array (const long double *v, size_t n);

GSL_FUN _gsl_vector_long_double_const_view
gsl_vector_long_double_const_view_array_with_stride (const long double *base,
                                               size_t stride,
                                               size_t n);

GSL_FUN _gsl_vector_long_double_view
gsl_vector_long_double_subvector (gsl_vector_long_double *v,
                            size_t i,
                            size_t n);

GSL_FUN _gsl_vector_long_double_view
gsl_vector_long_double_subvector_with_stride (gsl_vector_long_double *v,
                                        size_t i,
                                        size_t stride,
                                        size_t n);

GSL_FUN _gsl_vector_long_double_const_view
gsl_vector_long_double_const_subvector (const gsl_vector_long_double *v,
                                  size_t i,
                                  size_t n);

GSL_FUN _gsl_vector_long_double_const_view
gsl_vector_long_double_const_subvector_with_stride (const gsl_vector_long_double *v,
                                              size_t i,
                                              size_t stride,
                                              size_t n);

/* Operations */

GSL_FUN void gsl_vector_long_double_set_zero (gsl_vector_long_double * v);
GSL_FUN void gsl_vector_long_double_set_all (gsl_vector_long_double * v, long double x);
GSL_FUN int gsl_vector_long_double_set_basis (gsl_vector_long_double * v, size_t i);

GSL_FUN int gsl_vector_long_double_fread (FILE * stream, gsl_vector_long_double * v);
GSL_FUN int gsl_vector_long_double_fwrite (FILE * stream, const gsl_vector_long_double * v);
GSL_FUN int gsl_vector_long_double_fscanf (FILE * stream, gsl_vector_long_double * v);
GSL_FUN int gsl_vector_long_double_fprintf (FILE * stream, const gsl_vector_long_double * v,
                              const char *format);

GSL_FUN int gsl_vector_long_double_memcpy (gsl_vector_long_double * dest, const gsl_vector_long_double * src);

GSL_FUN int gsl_vector_long_double_reverse (gsl_vector_long_double * v);

GSL_FUN int gsl_vector_long_double_swap (gsl_vector_long_double * v, gsl_vector_long_double * w);
GSL_FUN int gsl_vector_long_double_swap_elements (gsl_vector_long_double * v, const size_t i, const size_t j);

GSL_FUN long double gsl_vector_long_double_max (const gsl_vector_long_double * v);
GSL_FUN long double gsl_vector_long_double_min (const gsl_vector_long_double * v);
GSL_FUN void gsl_vector_long_double_minmax (const gsl_vector_long_double * v, long double * min_out, long double * max_out);

GSL_FUN size_t gsl_vector_long_double_max_index (const gsl_vector_long_double * v);
GSL_FUN size_t gsl_vector_long_double_min_index (const gsl_vector_long_double * v);
GSL_FUN void gsl_vector_long_double_minmax_index (const gsl_vector_long_double * v, size_t * imin, size_t * imax);

GSL_FUN int gsl_vector_long_double_add (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_FUN int gsl_vector_long_double_sub (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_FUN int gsl_vector_long_double_mul (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_FUN int gsl_vector_long_double_div (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_FUN int gsl_vector_long_double_scale (gsl_vector_long_double * a, const double x);
GSL_FUN int gsl_vector_long_double_add_constant (gsl_vector_long_double * a, const double x);

GSL_FUN int gsl_vector_long_double_isnull (const gsl_vector_long_double * v);
GSL_FUN int gsl_vector_long_double_ispos (const gsl_vector_long_double * v);
GSL_FUN int gsl_vector_long_double_isneg (const gsl_vector_long_double * v);
GSL_FUN int gsl_vector_long_double_isnonneg (const gsl_vector_long_double * v);

GSL_FUN INLINE_DECL long double gsl_vector_long_double_get (const gsl_vector_long_double * v, const size_t i);
GSL_FUN INLINE_DECL void gsl_vector_long_double_set (gsl_vector_long_double * v, const size_t i, long double x);
GSL_FUN INLINE_DECL long double * gsl_vector_long_double_ptr (gsl_vector_long_double * v, const size_t i);
GSL_FUN INLINE_DECL const long double * gsl_vector_long_double_const_ptr (const gsl_vector_long_double * v, const size_t i);

#ifdef HAVE_INLINE

INLINE_FUN
long double
gsl_vector_long_double_get (const gsl_vector_long_double * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_VAL ("index out of range", GSL_EINVAL, 0);
    }
#endif
  return v->data[i * v->stride];
}

INLINE_FUN
void
gsl_vector_long_double_set (gsl_vector_long_double * v, const size_t i, long double x)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_VOID ("index out of range", GSL_EINVAL);
    }
#endif
  v->data[i * v->stride] = x;
}

INLINE_FUN
long double *
gsl_vector_long_double_ptr (gsl_vector_long_double * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_NULL ("index out of range", GSL_EINVAL);
    }
#endif
  return (long double *) (v->data + i * v->stride);
}

INLINE_FUN
const long double *
gsl_vector_long_double_const_ptr (const gsl_vector_long_double * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(i >= v->size))
    {
      GSL_ERROR_NULL ("index out of range", GSL_EINVAL);
    }
#endif
  return (const long double *) (v->data + i * v->stride);
}
#endif /* HAVE_INLINE */

__END_DECLS

#endif /* __GSL_VECTOR_LONG_DOUBLE_H__ */


#ifndef SPC_UTILS_H
#define SPC_UTILS_H

void init_search (const char *string);  /* Pbmsrch.C      */
char *strsearch (const char *string);   /* Pbmsrch.C      */

#include <stddef.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <limits.h>
#include <math.h>

#include <gsl/gsl_version.h>
#include <gsl/gsl_nan.h>
#include <gsl/gsl_sys.h>
#include <gsl/gsl_vector.h>

#include "aXe_errors.h"
#include "aXe_grism.h"

#define LAMBDACENTRAL 8000.0
#define COLNAMELENGTH 12
#define MAXCOLS       200

/**
 * Structure: sexcol
 * Structure for a column header
 */
typedef struct sexcol
{
  char name[COLNAMELENGTH];
  int  number;
}
sexcol;

/**
 * Structure: colinfo
 * Structure for the table header
 */
typedef struct colinfo
{
  int numcols;
  sexcol columns[MAXCOLS];
}
colinfo;

extern int
is_valid_entry(double mag);

extern int
get_valid_entries(const gsl_vector *magnitudes);

extern int
check_worldcoo_input(const colinfo * actcatinfo, const int thsky);

extern int
check_imagecoo_input(const colinfo * actcatinfo);

extern void
make_GOL_header(FILE *fout, const colinfo * actcatinfo,
                const gsl_vector * waves, const gsl_vector * cnums,
                const px_point backwin_cols, const px_point modinfo_cols);

extern colinfo *
get_sex_col_descr (char *filename);

extern double
get_col_value (const colinfo * actcatinfo, const char key[],
               gsl_vector * v, int fatal);

extern double
get_col_value2 (const colinfo * actcatinfo, const char key[],
                gsl_vector * v, int fatal);

extern int
line_is_valid (const colinfo * actcatinfo, char line[]);

extern int
get_magauto_col(const gsl_vector *wavelength, const gsl_vector *colnums,
                const double lambda_mark);

extern int
has_magnitudes(const colinfo * actcatinfo);

extern px_point
has_backwindow(const colinfo * actcatinfo);

extern px_point
has_modelinfo(const colinfo * actcatinfo);

extern int
get_magcols(const colinfo * actcatinfo, gsl_vector *wavelength,
            gsl_vector *colnums);

extern int
resolve_colname(const char colname[]);

extern void
get_columname(const colinfo * actcatinfo, const int colnum, char colname[]);

extern int
get_columnumber(const char cname[], const colinfo * actcatinfo);

extern char *
rmlead (char *str);

extern char *
stptok (const char *s, char *tok, size_t toklen, char *brk);

#define      strMove(d,s) memmove(d,s,strlen(s)+1)

extern void
lv1ws (char *str);

extern int
isnum2(char *string);

extern gsl_vector *
string_to_gsl_array (char *str);

extern void
check_libraries (void);

extern observation *
load_dummy_observation (void);
#endif


#pragma once

#include <map>
#include <set>

#ifdef _NOGSL
class gsl_vector;
#else
#include <gsl/gsl_vector.h>
#endif

class Interface;

class SymbolicEvaluator {
	double otherError = 1;
public:
	virtual void setInputs(Interface* inputValues_p) = 0;

    virtual void run(const gsl_vector* ctrls_p, const set<int>& nodesSubset) = 0;
    virtual void run(const gsl_vector* ctrls_p) = 0;

    virtual double getErrorOnConstraint(int nodeid, gsl_vector* grad) = 0;
    virtual double getErrorOnConstraint(int nodeid) = 0;

    virtual double getErrorForAsserts(const set<int>& nodeid, gsl_vector* grad) = 0;
	virtual double getErrorForAssert(int assertId, gsl_vector* grad)=0;

    virtual double dist(int nid) = 0;
    virtual double dist(int nid, gsl_vector* grad) = 0;

	virtual void print() = 0;
	virtual void printFull() = 0;

	void resetOtherError() {
		otherError = 1;
	}

	void setOtherError(double error) {
		otherError = error;
	}

	double getOtherError() {
		return otherError;
	}

	bool isSetOtherError() {
		return otherError != 1;
	}
};


/*
 * BaseMaster.h
 *
 *  Created on: Aug 7, 2014
 *      Author: thardin
 */

#ifndef BASEMASTER_H_
#define BASEMASTER_H_

#include "FMIClient.h"
#include <zmq.hpp>
#ifdef USE_GPL
#include <gsl/gsl_multiroots.h>
#include "common/fmigo_storage.h"
using namespace fmigo_storage;
#endif
#include <chrono>

namespace fmitcp_master {
    class BaseMaster {
        int rendezvous;
        //t1 = time at which to perform next step
        std::chrono::high_resolution_clock::time_point t1;

#ifdef USE_MPI
        std::string m_mpi_str;
#endif
    protected:
        std::vector<FMIClient*> m_clients;
        std::vector<WeakConnection> m_weakConnections;
        OutputRefsType clientWeakRefs;

        InputRefsValuesType initialNonReals;  //for loop solver

#ifdef USE_GPL
        class FmigoStorage m_fmigoStorage;//(std::vector<size_t>(0));
#endif

    public:

        zmq::socket_t rep_socket;
        bool initing, paused, running;
        bool zmqControl;
        int m_pendingRequests;
        double t; //current time

        explicit BaseMaster(zmq::context_t &context, std::vector<FMIClient*> clients, std::vector<WeakConnection> weakConnections);
        virtual ~BaseMaster() {
          info("%i rendezvous\n", rendezvous);
          int messages = 0;
          for(auto client: m_clients)
            messages += client->messages;
          info("%i messages\n", messages);
        }

#ifdef USE_GPL
        static int loop_residual_f(const gsl_vector *x, void *params, gsl_vector *f);

        inline FmigoStorage & get_storage(){return m_fmigoStorage;}
        void storage_alloc(const std::vector<FMIClient*> &clients){
            vector<size_t> states({});
            vector<size_t> indicators({});
            for(auto client: clients) {
                states.push_back(client->getNumContinuousStates());
                indicators.push_back(client->getNumEventIndicators());
            }
            get_storage().allocate_storage(states,indicators);
        }
#endif
      virtual std::string getFieldNames() const {return "";}
      virtual void writeFields(bool last, FILE *outfile) {}
        void solveLoops();
        virtual void prepare() {};
        virtual void runIteration(double t, double dt) = 0;

#define on(name) void name(FMIClient* slave) {}
        on(onSlaveInstantiated)
        on(onSlaveInitialized)
        on(onSlaveTerminated)
        on(onSlaveFreed)
        on(onSlaveStepped)
        on(onSlaveGotVersion)
        on(onSlaveSetReal)
        on(onSlaveGotState)
        on(onSlaveSetState)
        on(onSlaveFreedState)
        on(onSlaveDirectionalDerivative)

        //T is needed because func maybe of a function in Client (from which FMIClient is derived)
        void queueMessage(std::vector<FMIClient*> fmus, std::string str) {
            for (auto it = fmus.begin(); it != fmus.end(); it++) {
                (*it)->queueMessage(str);
            }
        }

        //like queueMessage() but only for one FMU
        void queueMessage(FMIClient *fmu, std::string str) {
            fmu->queueMessage(str);
        }

        //queueMessage() followed by wait() (blocking)
        void sendWait(std::vector<FMIClient*> fmus, std::string str) {
            queueMessage(fmus, str);
            wait();
        }

        //like sendWait() but only for one FMU (blocking)
        void sendWait(FMIClient *fmu, std::string str) {
            queueMessage(fmu, str);
            wait();
        }

        void wait();

        void resetT1();
        //wait for t1, advance t1 by timeStep
        void waitupT1(double timeStep);
        void handleZmqControl();

        void queueValueRequests();
        //if cset_set == true then only delete values for FMUs whose ID is in cset
        void deleteCachedValues(bool cset_set = false, const fmitcp::int_set& cset = fmitcp::int_set());
    };
};

#endif /* BASEMASTER_H_ */


/* Author:  G. Jungman */

/* Convenience header */
#ifndef __GSL_SPECFUNC_H__
#define __GSL_SPECFUNC_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <gsl/gsl_sf.h>

#endif /* __GSL_SPECFUNC_H__ */


/* matrix/gsl_matrix_complex_long_double.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Gerard Jungman, Brian Gough
 *
 * 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.
 */

#ifndef __GSL_MATRIX_COMPLEX_LONG_DOUBLE_H__
#define __GSL_MATRIX_COMPLEX_LONG_DOUBLE_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_vector_complex_long_double.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size1;
  size_t size2;
  size_t tda;
  long double * data;
  gsl_block_complex_long_double * block;
  int owner;
} gsl_matrix_complex_long_double ;

typedef struct
{
  gsl_matrix_complex_long_double matrix;
} _gsl_matrix_complex_long_double_view;

typedef _gsl_matrix_complex_long_double_view gsl_matrix_complex_long_double_view;

typedef struct
{
  gsl_matrix_complex_long_double matrix;
} _gsl_matrix_complex_long_double_const_view;

typedef const _gsl_matrix_complex_long_double_const_view gsl_matrix_complex_long_double_const_view;

/* Allocation */

gsl_matrix_complex_long_double *
gsl_matrix_complex_long_double_alloc (const size_t n1, const size_t n2);

gsl_matrix_complex_long_double *
gsl_matrix_complex_long_double_calloc (const size_t n1, const size_t n2);

gsl_matrix_complex_long_double *
gsl_matrix_complex_long_double_alloc_from_block (gsl_block_complex_long_double * b,
                                           const size_t offset,
                                           const size_t n1, const size_t n2, const size_t d2);

gsl_matrix_complex_long_double *
gsl_matrix_complex_long_double_alloc_from_matrix (gsl_matrix_complex_long_double * b,
                                            const size_t k1, const size_t k2,
                                            const size_t n1, const size_t n2);

gsl_vector_complex_long_double *
gsl_vector_complex_long_double_alloc_row_from_matrix (gsl_matrix_complex_long_double * m,
                                                const size_t i);

gsl_vector_complex_long_double *
gsl_vector_complex_long_double_alloc_col_from_matrix (gsl_matrix_complex_long_double * m,
                                                const size_t j);

void gsl_matrix_complex_long_double_free (gsl_matrix_complex_long_double * m);

/* Views */

_gsl_matrix_complex_long_double_view
gsl_matrix_complex_long_double_submatrix (gsl_matrix_complex_long_double * m,
                            const size_t i, const size_t j,
                            const size_t n1, const size_t n2);

_gsl_vector_complex_long_double_view
gsl_matrix_complex_long_double_row (gsl_matrix_complex_long_double * m, const size_t i);

_gsl_vector_complex_long_double_view
gsl_matrix_complex_long_double_column (gsl_matrix_complex_long_double * m, const size_t j);

_gsl_vector_complex_long_double_view
gsl_matrix_complex_long_double_diagonal (gsl_matrix_complex_long_double * m);

_gsl_vector_complex_long_double_view
gsl_matrix_complex_long_double_subdiagonal (gsl_matrix_complex_long_double * m, const size_t k);

_gsl_vector_complex_long_double_view
gsl_matrix_complex_long_double_superdiagonal (gsl_matrix_complex_long_double * m, const size_t k);

_gsl_vector_complex_long_double_view
gsl_matrix_complex_long_double_subrow (gsl_matrix_complex_long_double * m,
                                 const size_t i, const size_t offset,
                                 const size_t n);

_gsl_vector_complex_long_double_view
gsl_matrix_complex_long_double_subcolumn (gsl_matrix_complex_long_double * m,
                                    const size_t j, const size_t offset,
                                    const size_t n);

_gsl_matrix_complex_long_double_view
gsl_matrix_complex_long_double_view_array (long double * base,
                             const size_t n1,
                             const size_t n2);

_gsl_matrix_complex_long_double_view
gsl_matrix_complex_long_double_view_array_with_tda (long double * base,
                                      const size_t n1,
                                      const size_t n2,
                                      const size_t tda);

_gsl_matrix_complex_long_double_view
gsl_matrix_complex_long_double_view_vector (gsl_vector_complex_long_double * v,
                              const size_t n1,
                              const size_t n2);

_gsl_matrix_complex_long_double_view
gsl_matrix_complex_long_double_view_vector_with_tda (gsl_vector_complex_long_double * v,
                                       const size_t n1,
                                       const size_t n2,
                                       const size_t tda);

_gsl_matrix_complex_long_double_const_view
gsl_matrix_complex_long_double_const_submatrix (const gsl_matrix_complex_long_double * m,
                                  const size_t i, const size_t j,
                                  const size_t n1, const size_t n2);

_gsl_vector_complex_long_double_const_view
gsl_matrix_complex_long_double_const_row (const gsl_matrix_complex_long_double * m,
                            const size_t i);

_gsl_vector_complex_long_double_const_view
gsl_matrix_complex_long_double_const_column (const gsl_matrix_complex_long_double * m,
                               const size_t j);

_gsl_vector_complex_long_double_const_view
gsl_matrix_complex_long_double_const_diagonal (const gsl_matrix_complex_long_double * m);

_gsl_vector_complex_long_double_const_view
gsl_matrix_complex_long_double_const_subdiagonal (const gsl_matrix_complex_long_double * m,
                                    const size_t k);

_gsl_vector_complex_long_double_const_view
gsl_matrix_complex_long_double_const_superdiagonal (const gsl_matrix_complex_long_double * m,
                                      const size_t k);

_gsl_vector_complex_long_double_const_view
gsl_matrix_complex_long_double_const_subrow (const gsl_matrix_complex_long_double * m,
                                       const size_t i, const size_t offset,
                                       const size_t n);

_gsl_vector_complex_long_double_const_view
gsl_matrix_complex_long_double_const_subcolumn (const gsl_matrix_complex_long_double * m,
                                          const size_t j, const size_t offset,
                                          const size_t n);

_gsl_matrix_complex_long_double_const_view
gsl_matrix_complex_long_double_const_view_array (const long double * base,
                                   const size_t n1,
                                   const size_t n2);

_gsl_matrix_complex_long_double_const_view
gsl_matrix_complex_long_double_const_view_array_with_tda (const long double * base,
                                            const size_t n1,
                                            const size_t n2,
                                            const size_t tda);

_gsl_matrix_complex_long_double_const_view
gsl_matrix_complex_long_double_const_view_vector (const gsl_vector_complex_long_double * v,
                                    const size_t n1,
                                    const size_t n2);

_gsl_matrix_complex_long_double_const_view
gsl_matrix_complex_long_double_const_view_vector_with_tda (const gsl_vector_complex_long_double * v,
                                             const size_t n1,
                                             const size_t n2,
                                             const size_t tda);

/* Operations */

void gsl_matrix_complex_long_double_set_zero (gsl_matrix_complex_long_double * m);
void gsl_matrix_complex_long_double_set_identity (gsl_matrix_complex_long_double * m);
void gsl_matrix_complex_long_double_set_all (gsl_matrix_complex_long_double * m, gsl_complex_long_double x);

int gsl_matrix_complex_long_double_fread (FILE * stream, gsl_matrix_complex_long_double * m) ;
int gsl_matrix_complex_long_double_fwrite (FILE * stream, const gsl_matrix_complex_long_double * m) ;
int gsl_matrix_complex_long_double_fscanf (FILE * stream, gsl_matrix_complex_long_double * m);
int gsl_matrix_complex_long_double_fprintf (FILE * stream, const gsl_matrix_complex_long_double * m, const char * format);

int gsl_matrix_complex_long_double_memcpy(gsl_matrix_complex_long_double * dest, const gsl_matrix_complex_long_double * src);
int gsl_matrix_complex_long_double_swap(gsl_matrix_complex_long_double * m1, gsl_matrix_complex_long_double * m2);

int gsl_matrix_complex_long_double_swap_rows(gsl_matrix_complex_long_double * m, const size_t i, const size_t j);
int gsl_matrix_complex_long_double_swap_columns(gsl_matrix_complex_long_double * m, const size_t i, const size_t j);
int gsl_matrix_complex_long_double_swap_rowcol(gsl_matrix_complex_long_double * m, const size_t i, const size_t j);

int gsl_matrix_complex_long_double_transpose (gsl_matrix_complex_long_double * m);
int gsl_matrix_complex_long_double_transpose_memcpy (gsl_matrix_complex_long_double * dest, const gsl_matrix_complex_long_double * src);

int gsl_matrix_complex_long_double_equal (const gsl_matrix_complex_long_double * a, const gsl_matrix_complex_long_double * b);

int gsl_matrix_complex_long_double_isnull (const gsl_matrix_complex_long_double * m);
int gsl_matrix_complex_long_double_ispos (const gsl_matrix_complex_long_double * m);
int gsl_matrix_complex_long_double_isneg (const gsl_matrix_complex_long_double * m);
int gsl_matrix_complex_long_double_isnonneg (const gsl_matrix_complex_long_double * m);

int gsl_matrix_complex_long_double_add (gsl_matrix_complex_long_double * a, const gsl_matrix_complex_long_double * b);
int gsl_matrix_complex_long_double_sub (gsl_matrix_complex_long_double * a, const gsl_matrix_complex_long_double * b);
int gsl_matrix_complex_long_double_mul_elements (gsl_matrix_complex_long_double * a, const gsl_matrix_complex_long_double * b);
int gsl_matrix_complex_long_double_div_elements (gsl_matrix_complex_long_double * a, const gsl_matrix_complex_long_double * b);
int gsl_matrix_complex_long_double_scale (gsl_matrix_complex_long_double * a, const gsl_complex_long_double x);
int gsl_matrix_complex_long_double_add_constant (gsl_matrix_complex_long_double * a, const gsl_complex_long_double x);
int gsl_matrix_complex_long_double_add_diagonal (gsl_matrix_complex_long_double * a, const gsl_complex_long_double x);

/***********************************************************************/
/* The functions below are obsolete                                    */
/***********************************************************************/
int gsl_matrix_complex_long_double_get_row(gsl_vector_complex_long_double * v, const gsl_matrix_complex_long_double * m, const size_t i);
int gsl_matrix_complex_long_double_get_col(gsl_vector_complex_long_double * v, const gsl_matrix_complex_long_double * m, const size_t j);
int gsl_matrix_complex_long_double_set_row(gsl_matrix_complex_long_double * m, const size_t i, const gsl_vector_complex_long_double * v);
int gsl_matrix_complex_long_double_set_col(gsl_matrix_complex_long_double * m, const size_t j, const gsl_vector_complex_long_double * v);
/***********************************************************************/

/* inline functions if you are using GCC */

INLINE_DECL gsl_complex_long_double gsl_matrix_complex_long_double_get(const gsl_matrix_complex_long_double * m, const size_t i, const size_t j);
INLINE_DECL void gsl_matrix_complex_long_double_set(gsl_matrix_complex_long_double * m, const size_t i, const size_t j, const gsl_complex_long_double x);

INLINE_DECL gsl_complex_long_double * gsl_matrix_complex_long_double_ptr(gsl_matrix_complex_long_double * m, const size_t i, const size_t j);
INLINE_DECL const gsl_complex_long_double * gsl_matrix_complex_long_double_const_ptr(const gsl_matrix_complex_long_double * m, const size_t i, const size_t j);

#ifdef HAVE_INLINE

INLINE_FUN
gsl_complex_long_double
gsl_matrix_complex_long_double_get(const gsl_matrix_complex_long_double * m,
                     const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      gsl_complex_long_double zero = {{0,0}};

      if (i >= m->size1)
        {
          GSL_ERROR_VAL("first index out of range", GSL_EINVAL, zero) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_VAL("second index out of range", GSL_EINVAL, zero) ;
        }
    }
#endif
  return *(gsl_complex_long_double *)(m->data + 2*(i * m->tda + j)) ;
}

INLINE_FUN
void
gsl_matrix_complex_long_double_set(gsl_matrix_complex_long_double * m,
                     const size_t i, const size_t j, const gsl_complex_long_double x)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_VOID("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_VOID("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  *(gsl_complex_long_double *)(m->data + 2*(i * m->tda + j)) = x ;
}

INLINE_FUN
gsl_complex_long_double *
gsl_matrix_complex_long_double_ptr(gsl_matrix_complex_long_double * m,
                             const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  return (gsl_complex_long_double *)(m->data + 2*(i * m->tda + j)) ;
}

INLINE_FUN
const gsl_complex_long_double *
gsl_matrix_complex_long_double_const_ptr(const gsl_matrix_complex_long_double * m,
                                   const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  return (const gsl_complex_long_double *)(m->data + 2*(i * m->tda + j)) ;
}

#endif /* HAVE_INLINE */

__END_DECLS

#endif /* __GSL_MATRIX_COMPLEX_LONG_DOUBLE_H__ */


#if !defined(fvSupport_h)
#define fvSupport_h

#include <petsc.h>

/**
 * reproduces the petsc call with grad fixes for multiple fields
 * @param dm
 * @param fvm
 * @param locX
 * @param grad
 * @return
 */
PETSC_EXTERN PetscErrorCode DMPlexGetDataFVM_MulfiField(DM dm, PetscFV fv, Vec *cellgeom, Vec *facegeom, DM *gradDM);

/**
 * Check to make sure local ghost boundary have valid gradient values
 */
PETSC_EXTERN PetscErrorCode ABLATE_FillGradientBoundary(DM dm, PetscFV auxFvm, Vec localXVec, Vec gradLocalVec);

#endif


/**
 * This file is part of the Contour terminal project
 *   Copyright (c) 2019-2021 Christian Parpart <christian@parpart.family>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#include <gsl/span>
#include <gsl/span_ext>

#include <algorithm>
#include <cassert>
#include <iterator>
#include <stdexcept>
#include <vector>

namespace crispy
{

template <typename T, typename Vector>
struct RingIterator;
template <typename T, typename Vector>
struct RingReverseIterator;

/**
 * Implements an efficient ring buffer over type T
 * and the underlying storage Vector.
 */
template <typename T, typename Vector = std::vector<T>>
class basic_ring
{
  public:
    using value_type = T;
    using iterator = RingIterator<value_type, Vector>;
    using const_iterator = RingIterator<value_type const, Vector>;
    using reverse_iterator = RingReverseIterator<value_type, Vector>;
    using const_reverse_iterator = RingReverseIterator<value_type const, Vector>;
    using difference_type = long;
    using offset_type = long;

    basic_ring() = default;
    basic_ring(basic_ring const&) = default;
    basic_ring& operator=(basic_ring const&) = default;
    basic_ring(basic_ring&&) noexcept = default;
    basic_ring& operator=(basic_ring&&) noexcept = default;
    virtual ~basic_ring() = default;

    explicit basic_ring(Vector storage): _storage(std::move(storage)) {}

    value_type const& operator[](offset_type i) const noexcept
    {
        return _storage[size_t(offset_type(_zero + size()) + i) % size()];
    }
    value_type& operator[](offset_type i) noexcept
    {
        return _storage[size_t(offset_type(_zero + size()) + i) % size()];
    }

    value_type const& at(offset_type i) const noexcept
    {
        return _storage[size_t(_zero + size() + i) % size()];
    }
    value_type& at(offset_type i) noexcept
    {
        return _storage[size_t(offset_type(_zero + size()) + i) % size()];
    }

    Vector& storage() noexcept { return _storage; }
    Vector const& storage() const noexcept { return _storage; }
    std::size_t zero_index() const noexcept { return _zero; }

    void rezero(iterator i);
    void rezero();

    std::size_t size() const noexcept { return _storage.size(); }

    // positvie count rotates right, negative count rotates left
    void rotate(int count) noexcept { _zero = size_t(offset_type(_zero + size()) - count) % size(); }

    void rotate_left(std::size_t count) noexcept { _zero = (_zero + size() + count) % size(); }
    void rotate_right(std::size_t count) noexcept { _zero = (_zero + size() - count) % size(); }
    void unrotate() { _zero = 0; }

    value_type& front() noexcept { return at(0); }
    value_type const& front() const noexcept { return at(0); }

    value_type& back()
    {
        if (size() == 0)
            throw std::length_error("empty");

        return at(static_cast<offset_type>(size()) - 1);
    }

    value_type const& back() const
    {
        if (size() == 0)
            throw std::length_error("empty");

        return at(static_cast<offset_type>(size()) - 1);
    }

    iterator begin() noexcept { return iterator { this, 0 }; }
    iterator end() noexcept { return iterator { this, static_cast<difference_type>(size()) }; }

    const_iterator cbegin() const noexcept
    {
        return const_iterator { (basic_ring<value_type const, Vector>*) this, 0 };
    }
    const_iterator cend() const noexcept
    {
        return const_iterator { (basic_ring<value_type const, Vector>*) this,
                                static_cast<difference_type>(size()) };
    }

    const_iterator begin() const noexcept { return cbegin(); }
    const_iterator end() const noexcept { return cend(); }

    reverse_iterator rbegin() noexcept;
    reverse_iterator rend() noexcept;

    const_reverse_iterator rbegin() const noexcept;
    const_reverse_iterator rend() const noexcept;

    gsl::span<value_type> span(offset_type start, size_t count) noexcept
    {
        auto a = std::next(begin(), start);
        auto b = std::next(a, count);
        return gsl::make_span(a, b);
    }

    gsl::span<value_type const> span(offset_type start, size_t count) const noexcept
    {
        auto a = std::next(begin(), start);
        auto b = std::next(a, count);
        return gsl::make_span(a, b);
    }

  protected:
    Vector _storage;
    std::size_t _zero = 0;
};

/**
 * Implements an efficient ring buffer over type T
 * and the underlying dynamic storage type Vector<T, Allocator>.
 */
template <typename T,
          template <typename, typename> class Container = std::vector,
          typename Allocator = std::allocator<T>>
class ring: public basic_ring<T, Container<T, Allocator>>
{
  public:
    using basic_ring<T, Container<T, Allocator>>::basic_ring;

    ring(size_t capacity, T value): ring(Container<T, Allocator>(capacity, value)) {}
    explicit ring(size_t capacity): ring(capacity, T {}) {}

    size_t size() const noexcept { return this->_storage.size(); }

    void reserve(size_t capacity) { this->_storage.reserve(capacity); }
    void resize(size_t newSize)
    {
        this->rezero();
        this->_storage.resize(newSize);
    }
    void clear()
    {
        this->_storage.clear();
        this->_zero = 0;
    }
    void push_back(T const& _value) { this->_storage.push_back(_value); }

    void push_back(T&& _value) { this->emplace_back(std::move(_value)); }

    template <typename... Args>
    void emplace_back(Args&&... args)
    {
        this->_storage.emplace_back(std::forward<Args>(args)...);
    }

    void pop_front() { this->_storage.erase(this->_storage.begin()); }
};

/// Fixed-size basic_ring<T> implementation
template <typename T, std::size_t N>
using fixed_size_ring = basic_ring<T, std::array<T, N>>;

// {{{ iterator
template <typename T, typename Vector>
struct RingIterator
{
    using iterator_category = std::random_access_iterator_tag;
    using value_type = T;
    using difference_type = long;
    using pointer = T*;
    using reference = T&;

    basic_ring<T, Vector>* ring {};
    difference_type current {};

    RingIterator(basic_ring<T, Vector>* aRing, difference_type aCurrent): ring { aRing }, current { aCurrent }
    {
    }

    RingIterator() = default;

    RingIterator(RingIterator const&) = default;
    RingIterator& operator=(RingIterator const&) = default;

    RingIterator(RingIterator&&) noexcept = default;
    RingIterator& operator=(RingIterator&&) noexcept = default;

    RingIterator& operator++() noexcept
    {
        ++current;
        return *this;
    }

    RingIterator operator++(int) noexcept
    {
        auto old = *this;
        ++(*this);
        return old;
    }

    RingIterator& operator--() noexcept
    {
        --current;
        return *this;
    }

    RingIterator operator--(int) noexcept
    {
        auto old = *this;
        --(*this);
        return old;
    }

    RingIterator& operator+=(int n) noexcept
    {
        current += n;
        return *this;
    }
    RingIterator& operator-=(int n) noexcept
    {
        current -= n;
        return *this;
    }

    RingIterator operator+(difference_type n) noexcept { return RingIterator { ring, current + n }; }
    RingIterator operator-(difference_type n) noexcept { return RingIterator { ring, current - n }; }

    RingIterator operator+(RingIterator const& rhs) const noexcept
    {
        return RingIterator { ring, current + rhs.current };
    }
    difference_type operator-(RingIterator const& rhs) const noexcept { return current - rhs.current; }

    friend RingIterator operator+(difference_type n, RingIterator a)
    {
        return RingIterator { a.ring, n + a.current };
    }
    friend RingIterator operator-(difference_type n, RingIterator a)
    {
        return RingIterator { a.ring, n - a.current };
    }

    bool operator==(RingIterator const& rhs) const noexcept { return current == rhs.current; }
    bool operator!=(RingIterator const& rhs) const noexcept { return current != rhs.current; }

    T& operator*() noexcept { return (*ring)[current]; }
    T const& operator*() const noexcept { return (*ring)[current]; }

    T* operator->() noexcept { return &(*ring)[current]; }
    T* operator->() const noexcept { return &(*ring)[current]; }
};
// }}}

// {{{ reverse iterator
template <typename T, typename Vector>
struct RingReverseIterator
{
    using iterator_category = std::random_access_iterator_tag;
    using value_type = T;
    using difference_type = long;
    using pointer = T*;
    using reference = T&;

    basic_ring<T, Vector>* ring;
    difference_type current;

    RingReverseIterator(basic_ring<T, Vector>* _ring, difference_type _current):
        ring { _ring }, current { _current }
    {
    }

    RingReverseIterator(RingReverseIterator const&) = default;
    RingReverseIterator& operator=(RingReverseIterator const&) = default;

    RingReverseIterator(RingReverseIterator&&) noexcept = default;
    RingReverseIterator& operator=(RingReverseIterator&&) noexcept = default;

    RingReverseIterator& operator++() noexcept
    {
        ++current;
        return *this;
    }
    RingReverseIterator& operator++(int) noexcept { return ++(*this); }

    RingReverseIterator& operator--() noexcept
    {
        --current;
        return *this;
    }
    RingReverseIterator& operator--(int) noexcept { return --(*this); }

    RingReverseIterator& operator+=(int n) noexcept
    {
        current += n;
        return *this;
    }
    RingReverseIterator& operator-=(int n) noexcept
    {
        current -= n;
        return *this;
    }

    RingReverseIterator operator+(difference_type n) noexcept
    {
        return RingReverseIterator { ring, current + n };
    }
    RingReverseIterator operator-(difference_type n) noexcept
    {
        return RingReverseIterator { ring, current - n };
    }

    RingReverseIterator operator+(RingReverseIterator const& rhs) const noexcept
    {
        return RingReverseIterator { ring, current + rhs.current };
    }
    difference_type operator-(RingReverseIterator const& rhs) const noexcept { return current - rhs.current; }

    friend RingReverseIterator operator+(difference_type n, RingReverseIterator a)
    {
        return RingReverseIterator { a.ring, n + a.current };
    }
    friend RingReverseIterator operator-(difference_type n, RingReverseIterator a)
    {
        return RingReverseIterator { a.ring, n - a.current };
    }

    bool operator==(RingReverseIterator const& rhs) const noexcept { return current == rhs.current; }
    bool operator!=(RingReverseIterator const& rhs) const noexcept { return current != rhs.current; }

    T& operator*() noexcept { return (*ring)[ring->size() - current - 1]; }
    T const& operator*() const noexcept { return (*ring)[ring->size() - current - 1]; }

    T* operator->() noexcept { return &(*ring)[static_cast<difference_type>(ring->size()) - current - 1]; }

    T* operator->() const noexcept
    {
        return &(*ring)[static_cast<difference_type>(ring->size()) - current - 1];
    }
};
// }}}

// {{{ basic_ring<T> impl
template <typename T, typename Vector>
typename basic_ring<T, Vector>::reverse_iterator basic_ring<T, Vector>::rbegin() noexcept
{
    return reverse_iterator { this, 0 };
}

template <typename T, typename Vector>
typename basic_ring<T, Vector>::reverse_iterator basic_ring<T, Vector>::rend() noexcept
{
    return reverse_iterator { this, size() };
}

template <typename T, typename Vector>
typename basic_ring<T, Vector>::const_reverse_iterator basic_ring<T, Vector>::rbegin() const noexcept
{
    return const_reverse_iterator { (basic_ring<T const, Vector>*) this, 0 };
}

template <typename T, typename Vector>
typename basic_ring<T, Vector>::const_reverse_iterator basic_ring<T, Vector>::rend() const noexcept
{
    return const_reverse_iterator { (basic_ring<T const, Vector>*) this,
                                    static_cast<difference_type>(size()) };
}

template <typename T, typename Vector>
void basic_ring<T, Vector>::rezero()
{
    std::rotate(begin(), std::next(begin(), static_cast<difference_type>(_zero)), end()); // shift-left
    _zero = 0;
}

template <typename T, typename Vector>
void basic_ring<T, Vector>::rezero(iterator i)
{
    std::rotate(begin(), std::next(begin(), i.current), end()); // shift-left
    _zero = 0;
}
// }}}

} // namespace crispy