#include <mpi.h>
#include <Base/Base.h>
#include <Discretisation/Geometry/Geometry.h>

#include "types.h"
#include "ShapeClass.h"
#include "SurfaceShape.h"

#include <assert.h>
#include <string.h>
#include <math.h>

#define EPSILON  0.0000001
#define TWOPI 6.283185307179586476925287
#define RTOD 57.2957795

/* Textual name of this class */
const Type SurfaceShape_Type = "SurfaceShape";

/*----------------------------------------------------------------------------------------------------------------------------------
** Constructors
*/

SurfaceShape* SurfaceShape_New(
		Name                                  name,
		Dimension_Index                       dim,
		XYZ                                   centre,
		double                                alpha,
		double                                beta,
		double                                gamma,
		Coord_List                            vertexList1,
		Coord_List                            vertexList2,
		Index                                 vertexCount,
		Index 								  nx,
		Index 								  ny)
{
	SurfaceShape* self = (SurfaceShape*)_SurfaceShape_DefaultNew( name );

	SurfaceShape_InitAll(
		self,
		dim,
		centre,
		alpha,
		beta,
		gamma,
		vertexList1,
		vertexList2,
		vertexCount,
		nx,
		ny);
	return self;
}

SurfaceShape* _SurfaceShape_New(
		SizeT                                 _sizeOfSelf,
		Type                                  type,
		Stg_Class_DeleteFunction*             _delete,
		Stg_Class_PrintFunction*              _print,
		Stg_Class_CopyFunction*               _copy,
		Stg_Component_DefaultConstructorFunction* _defaultConstructor,
		Stg_Component_ConstructFunction*      _construct,
		Stg_Component_BuildFunction*          _build,
		Stg_Component_InitialiseFunction*     _initialise,
		Stg_Component_ExecuteFunction*        _execute,
		Stg_Component_DestroyFunction*        _destroy,
		Stg_Shape_IsCoordInsideFunction*      _isCoordInside,
		Stg_Shape_CalculateVolumeFunction*    _calculateVolume,
		Name                                  name )
{
	SurfaceShape* self;

	/* Allocate memory */
	assert( _sizeOfSelf >= sizeof(SurfaceShape) );
	self = (SurfaceShape*)_Stg_Shape_New(
			_sizeOfSelf,
			type,
			_delete,
			_print,
			_copy,
			_defaultConstructor,
			_construct,
			_build,
			_initialise,
			_execute,
			_destroy,
			_isCoordInside ,
			_calculateVolume,
			name );

	/* General info */

	/* Virtual Info */
	self->_isCoordInside = _isCoordInside;

	return self;
}

void _SurfaceShape_Init( void* polygon, Coord_List vertexList1, Coord_List vertexList2, Index vertexCount, Index nx, Index ny ) {
	SurfaceShape* self = (SurfaceShape*)polygon;

	self->vertexList1 = Memory_Alloc_Array( Coord, vertexCount, "vertexList1" );
	self->vertexList2 = Memory_Alloc_Array( Coord, vertexCount, "vertexList2" );
	memcpy( self->vertexList1 , vertexList1, sizeof(Coord) * vertexCount );
	memcpy( self->vertexList2 , vertexList2, sizeof(Coord) * vertexCount );
	self->vertexCount = vertexCount;
	self->nx = nx;
	self->ny = ny;
}


void SurfaceShape_InitAll(
		void*                                 polygon,
		Dimension_Index                       dim,
		Coord                                 centre,
		double                                alpha,
		double                                beta,
		double                                gamma,
		Coord_List                            vertexList1,
		Coord_List                            vertexList2,
		Index                                 vertexCount,
		Index								  nx,
		Index								  ny)
{
	SurfaceShape* self = (SurfaceShape*)polygon;
	Stg_Shape_InitAll( self, dim, centre, alpha, beta, gamma);
	_SurfaceShape_Init( self, vertexList1, vertexList2, vertexCount, nx, ny );
}


/*------------------------------------------------------------------------------------------------------------------------
** Virtual functions
*/

void _SurfaceShape_Delete( void* polygon ) {
	SurfaceShape*       self = (SurfaceShape*)polygon;
	Coord_List     vertexList1 = self->vertexList1;
	Coord_List     vertexList2 = self->vertexList2;

	Memory_Free( vertexList1 );
	Memory_Free( vertexList2 );


	/* Delete parent */
	_Stg_Shape_Delete( self );
}


void _SurfaceShape_Print( void* polygon, Stream* stream ) {
	SurfaceShape* self = (SurfaceShape*)polygon;

	/* Print parent */
	_Stg_Shape_Print( self, stream );
}



void* _SurfaceShape_Copy( void* polygon, void* dest, Bool deep, Name nameExt, PtrMap* ptrMap ) {
	SurfaceShape*	self = (SurfaceShape*)polygon;
	SurfaceShape*	newSurfaceShape;

	newSurfaceShape = (SurfaceShape*)_Stg_Shape_Copy( self, dest, deep, nameExt, ptrMap );

	newSurfaceShape->vertexList1 = Memory_Alloc_Array( Coord, self->vertexCount, "vertexList1" );
	newSurfaceShape->vertexList2 = Memory_Alloc_Array( Coord, self->vertexCount, "vertexList2" );
	memcpy( newSurfaceShape->vertexList1 , self->vertexList1, sizeof(Coord) * self->vertexCount );
	memcpy( newSurfaceShape->vertexList2 , self->vertexList2, sizeof(Coord) * self->vertexCount );

	newSurfaceShape->vertexList1  = self->vertexList1;
	newSurfaceShape->vertexList2  = self->vertexList2;
	newSurfaceShape->vertexCount = self->vertexCount;
	newSurfaceShape->nx = self->nx;
	newSurfaceShape->ny = self->ny;

	return (void*)newSurfaceShape;
}

void* _SurfaceShape_DefaultNew( Name name ) {
	return (void*) _SurfaceShape_New(
			sizeof(SurfaceShape),
			SurfaceShape_Type,
			_SurfaceShape_Delete,
			_SurfaceShape_Print,
			_SurfaceShape_Copy,
			_SurfaceShape_DefaultNew,
			_SurfaceShape_Construct,
			_SurfaceShape_Build,
			_SurfaceShape_Initialise,
			_SurfaceShape_Execute,
			_SurfaceShape_Destroy,
			_SurfaceShape_IsCoordInside,
			_SurfaceShape_CalculateVolume,
			name );
}


void _SurfaceShape_Construct( void* polygon, Stg_ComponentFactory* cf, void* data ) {
	SurfaceShape*           self       = (SurfaceShape*)polygon;
	Index                   vertexCount;
	Index                   vertexCount2;
	Index                   vertex_I;
	Coord_List              vertexList1;
	Coord_List              vertexList2;
	Index 					nx;
	Index					ny;
	FILE*					fp;
	Name 					surfaceName1;
	Name 					surfaceName2;
	char 					line[1000];
	double*                 coord;
	Stream*                 stream      = cf->infoStream;
	Stream*                 errorStream = Journal_Register( Error_Type, self->type );

	_Stg_Shape_Construct( self, cf, data );

	surfaceName1 = Stg_ComponentFactory_GetString( cf, self->name, "surfaceName1", "" );
	surfaceName2 = Stg_ComponentFactory_GetString( cf, self->name, "surfaceName2", "" );
	nx = Stg_ComponentFactory_GetInt( cf, self->name, "nx", 0 );
	ny = Stg_ComponentFactory_GetInt( cf, self->name, "ny", 0 );

	/* First surface which is the bottom one */
	fp = fopen( surfaceName1, "r" );
	if ( !fp ) {
	   printf("Can not open the file %s\n",surfaceName1);
	   abort();
	}
	vertexCount=0;
	while ( !feof( fp ) ) {
		fgets( line, 1000, fp );
		vertexCount++ ;
	}

	Journal_Firewall( vertexCount >= 3, errorStream,
			"To few verticies given in trying to build shape '%s' named '%s'.\n"
			"A surface needs at least three verticies.\n",
			self->type, self->name );

	rewind( fp ) ;

	/* Allocate space */
	vertexList1 = Memory_Alloc_Array( Coord , vertexCount, "Vertex Array" );
	memset( vertexList1, 0, vertexCount * sizeof(Coord) );

	Stream_Indent( stream );
	double pt[ 3 ];
	for ( vertex_I = 0; vertex_I < vertexCount; vertex_I++ ) {
		fscanf( fp, "%lf %lf %lf", &pt[ 0 ], &pt[ 1 ], &pt[ 2 ] );
		vertexList1[ vertex_I ][ 0 ] = pt[ 0 ] ;
		vertexList1[ vertex_I ][ 1 ] = pt[ 1 ] ;
		vertexList1[ vertex_I ][ 2 ] = pt[ 2 ] ;
		coord = vertexList1[ vertex_I ];
		/* Print Position */
		Journal_PrintfL( stream, 2, "(%0.3g, %0.3g, %0.3g)\n", coord[ I_AXIS ], coord[ J_AXIS ], coord[ K_AXIS ] );

    }
    fclose( fp ) ;

    /* Second surface which is the top one */
	fp = fopen( surfaceName2, "r" );
	if ( !fp ) {
	   printf("Can not open the file %s\n",surfaceName2);
	   abort();
	}
	vertexCount2=0;
	while ( !feof( fp ) ) {
		fgets( line, 1000, fp );
		vertexCount2++ ;
	}

	Journal_Firewall( vertexCount2 >= 3, errorStream,
			"To few verticies given in trying to build shape '%s' named '%s'.\n"
			"A surface needs at least three verticies.\n",
			self->type, self->name );

	if( vertexCount2 != vertexCount ) {
		printf("Points number differs between %s surfaces %s\n",surfaceName1,surfaceName2);
		abort();
	}

	rewind( fp ) ;

	/* Allocate space */
	vertexList2 = Memory_Alloc_Array( Coord , vertexCount, "Vertex Array" );
	memset( vertexList2, 0, vertexCount * sizeof(Coord) );

	Stream_Indent( stream );
	for ( vertex_I = 0; vertex_I < vertexCount; vertex_I++ ) {
		fscanf( fp, "%lf %lf %lf", &pt[ 0 ], &pt[ 1 ], &pt[ 2 ] );
		vertexList2[ vertex_I ][ 0 ] = pt[ 0 ] ;
		vertexList2[ vertex_I ][ 1 ] = pt[ 1 ] ;
		vertexList2[ vertex_I ][ 2 ] = pt[ 2 ] ;
		coord = vertexList2[ vertex_I ];
		/* Print Position */
		Journal_PrintfL( stream, 2, "(%0.3g, %0.3g, %0.3g)\n", coord[ I_AXIS ], coord[ J_AXIS ], coord[ K_AXIS ] );

    }
    fclose( fp ) ;


	Stream_UnIndent( stream );

	_SurfaceShape_Init( self, vertexList1, vertexList2, vertexCount, nx, ny );

	Memory_Free( vertexList1 );
	Memory_Free( vertexList2 );
}

void _SurfaceShape_Build( void* polygon, void* data ) {
	SurfaceShape*	self = (SurfaceShape*)polygon;

	_Stg_Shape_Build( self, data );
}
void _SurfaceShape_Initialise( void* polygon, void* data ) {
	SurfaceShape*	self = (SurfaceShape*)polygon;

	_Stg_Shape_Initialise( self, data );
}
void _SurfaceShape_Execute( void* polygon, void* data ) {
	SurfaceShape*	self = (SurfaceShape*)polygon;

	_Stg_Shape_Execute( self, data );
}
void _SurfaceShape_Destroy( void* polygon, void* data ) {
	SurfaceShape*	self = (SurfaceShape*)polygon;

	_Stg_Shape_Destroy( self, data );
}

/*--------------------------------------------------------------------------------------------------------------------------
** Public Functions
*/

/*---------------------------------------------------------------------------------------------------------------------
** Private Member functions
*/
void _SurfaceShape_invMatrix(double mat[ 3 ][ 3 ], double dest[ 3 ][ 3 ])
{
	Index 				Row;
	Index				Col;
	double 				trans[ 3 ][ 3 ];
	double 				inv[ 3 ][ 3 ];
	double 				det			=0;
	double 				inv_det;

	/* Determinant */
	for ( Row = 0, Col = 0; Col < 3; Col++ ) {
		if ( Col == 2 )
			det += mat[ Row ][ Col ] * mat[ Row+1 ][ 0 ] * mat[ Row+2 ][ 1 ] ;
		else if ( Col == 1 )
			det += mat[ Row ][ Col ] * mat[ Row+1 ][ Col+1 ] * mat[ Row+2 ][ 0 ] ;
		else
			det += mat[ Row ][ Col ] * mat[ Row+1 ][ Col+1 ] * mat[ Row+2 ][ Col+2 ] ;
	}
	for ( Row = 2, Col = 0; Col < 3; Col++ ) {
		if ( Col == 2 )
			det -= mat[ Row ][ Col ] * mat[ Row-1 ][ 0 ] * mat[ Row-2 ][ 1 ] ;
		else if ( Col == 1 )
			det -= mat[ Row ][ Col ] * mat[ Row-1 ][ Col+1 ] * mat[ Row-2 ][ 0 ] ;
		else
			det -= mat[ Row ][ Col ] * mat[ Row-1 ][ Col+1 ] * mat[ Row-2 ][ Col+2 ] ;
	}

	if ( det != 0 )
		inv_det = 1.0/det ;
	else {
		printf( "Cannot inverse this matrix -> determinant null) when creating Surface shape.\n" ) ;
		abort() ;
	}

	/** Transpose */
	for ( Row = 0; Row < 3; Row++ )
		for ( Col = 0; Col < 3; Col++ )
			trans[ Row ][ Col ] = mat[ Col ][ Row ];

	/** Inverse */
	inv[ 0 ][ 0 ] = trans[ 1 ][ 1 ] * trans[ 2 ][ 2 ] - ( trans[ 2 ][ 1 ] * trans[ 1 ][ 2 ] ) ;
	inv[ 0 ][ 1 ] = ( -1 ) * ( trans[ 1 ][ 0 ] * trans[ 2 ][ 2 ] - (trans[ 2 ][ 0 ] * trans[ 1 ][ 2 ] ) ) ;
	inv[ 0 ][ 2 ] = trans[ 1 ][ 0 ] * trans[ 2 ][ 1 ] - ( trans[ 2 ][ 0 ] * trans[ 1 ][ 1 ] ) ;

	inv[ 1 ][ 0 ] = ( -1 ) * ( trans[ 0 ][ 1 ] * trans[ 2 ][ 2 ] - trans[ 2 ][ 1 ] * trans[ 0 ][ 2 ] ) ;
	inv[ 1 ][ 1 ] = trans[ 0 ][ 0 ] * trans[ 2 ][ 2 ] - trans[ 2 ][ 0 ] * trans[ 0 ][ 2 ] ;
	inv[ 1 ][ 2 ] = ( -1 ) * ( trans[ 0 ][ 0 ] * trans[ 2 ][ 1 ] - trans[ 2 ][ 0 ] * trans[ 0 ][ 1 ] ) ;

	inv[ 2 ][ 0 ] = trans[ 0 ][ 1 ] * trans[ 1 ][ 2 ] - trans[ 1 ][ 1 ] * trans[ 0 ][ 2 ] ;
	inv[ 2 ][ 1 ] = ( -1 ) * ( trans[ 0 ][ 0 ] * trans[ 1 ][ 2 ] - trans[ 1 ][ 0 ] * trans[ 0 ][ 2 ] ) ;
	inv[ 2 ][ 2 ] = trans[ 0 ][ 0 ] * trans[ 1 ][ 1 ] - trans[ 1 ][ 0 ] * trans[ 0 ][ 1 ] ;

	// Inverse
	for ( Row = 0; Row < 3; Row++)
		for ( Col = 0; Col < 3; Col++)
			dest[ Row ][ Col ] = inv[ Row ][ Col ] * inv_det ;

}

/* Algorithm describes Ronald Peikert's page http://graphics.ethz.ch/~peikert/personal/HexCellTest/index.html
 *
 * Algorithm works by translating and transforming each hexahedral cell faces and check if the point is above
 * the considered face */
Bool _SurfaceShape_IsCoordInside( void* polygon, Coord coord ) {
	SurfaceShape*        self                = (SurfaceShape*) polygon;
	Index           vertexCount         = self->vertexCount;
	Index			nx					= self->nx;
	Index			ny					= self->ny;
	Index  			vert_Index 			= 0;
	Coord_List      vertexList1         = self->vertexList1;
	Coord_List      vertexList2         = self->vertexList2;
	Index           vertex_I;
	Index           vertex_J;
	Index			face_Index;
	Coord           newCoord;

	/* Transform coordinate into canonical reference frame */
	Stg_Shape_TransformCoord( self, coord, newCoord );

	double XX[ nx ][ ny ];
	double YY[ nx ][ ny ];
	double Z1[ nx ][ ny ];
	double Z2[ nx ][ ny ];
	double minZ;
	double maxZ;

	int 			i, j, k;
	double 			h, sum, slp;
	XYZ 			A, A1, A2;
	XYZ				B, B1, B2;
	XYZ 			D, D1, D2;
	XYZ				C, C1, C2;
	XYZ				P1, P2, E;
	Bool			Inside[2];
	double			p[ 8 ][ 3 ], X[ 3 ][ 3 ], H[ 3 ][ 3 ], Y[ 3 ][ 3 ], T[ 3 ][ 3 ];

	for ( vertex_I = 0; vertex_I < nx; vertex_I++ )
		for ( vertex_J = 0; vertex_J < ny; vertex_J++ ) {
			XX[ vertex_I ][ vertex_J ] = vertexList1[ vert_Index ][ 0 ];
			YY[ vertex_I ][ vertex_J ] = vertexList1[ vert_Index ][ 1 ];
			Z1[ vertex_I ][ vertex_J ] = vertexList1[ vert_Index ][ 2 ];
			Z2[ vertex_I ][ vertex_J ] = vertexList2[ vert_Index ][ 2 ];
			vert_Index++;
		}

	for ( vertex_I = 0; vertex_I < nx-1; vertex_I++ )
		for ( vertex_J = 0; vertex_J < ny-1; vertex_J++ ) {
			/* Find the appropriate X,Y coordinate which contains the point */
			if ( ( XX[ vertex_I ][ vertex_J ] <= newCoord[ I_AXIS ] ) && ( XX[ vertex_I+1 ][ vertex_J   ] > newCoord[ I_AXIS ] ) &&
			     ( YY[ vertex_I ][ vertex_J ] <= newCoord[ K_AXIS ] ) && ( YY[ vertex_I   ][ vertex_J+1 ] > newCoord[ K_AXIS ] ) ) {

					/* Coarse check to see if the Z coordinate of the point is between the volume minZ and maxZ */
					minZ = Z1[ vertex_I ][ vertex_J ] ;
					if ( minZ > Z1[   vertex_I ][ vertex_J+1 ] ) minZ = Z1[   vertex_I ][ vertex_J+1 ];
					if ( minZ > Z1[ vertex_I+1 ][ vertex_J+1 ] ) minZ = Z1[ vertex_I+1 ][ vertex_J+1 ];
					if ( minZ > Z1[ vertex_I+1 ][   vertex_J ] ) minZ = Z1[ vertex_I+1 ][   vertex_J ];

					maxZ = Z2[ vertex_I ][ vertex_J ] ;
					if ( maxZ < Z2[   vertex_I ][ vertex_J+1 ] ) maxZ = Z2[   vertex_I ][ vertex_J+1 ];
					if ( maxZ < Z2[ vertex_I+1 ][ vertex_J+1 ] ) maxZ = Z2[ vertex_I+1 ][ vertex_J+1 ];
					if ( maxZ < Z2[ vertex_I+1 ][   vertex_J ] ) maxZ = Z2[ vertex_I+1 ][   vertex_J ];

					if ( ( newCoord[ J_AXIS ] < minZ ) || ( newCoord[ J_AXIS ] > maxZ ) )
						return False;
					else {
						/* Bottom face vertices */
						p[ 0 ][ 0 ] = XX[ vertex_I   ][ vertex_J   ] ; p[ 0 ][ 1 ] = YY[ vertex_I   ][ vertex_J   ] ; p[ 0 ][ 2 ] = Z1[ vertex_I   ][ vertex_J   ] ;
						p[ 1 ][ 0 ] = XX[ vertex_I+1 ][ vertex_J   ] ; p[ 1 ][ 1 ] = YY[ vertex_I+1 ][ vertex_J   ] ; p[ 1 ][ 2 ] = Z1[ vertex_I+1 ][ vertex_J   ] ;
						p[ 2 ][ 0 ] = XX[ vertex_I+1 ][ vertex_J+1 ] ; p[ 2 ][ 1 ] = YY[ vertex_I+1 ][ vertex_J+1 ] ; p[ 2 ][ 2 ] = Z1[ vertex_I+1 ][ vertex_J+1 ] ;
						p[ 3 ][ 0 ] = XX[ vertex_I   ][ vertex_J+1 ] ; p[ 3 ][ 1 ] = YY[ vertex_I   ][ vertex_J+1 ] ; p[ 3 ][ 2 ] = Z1[ vertex_I   ][ vertex_J+1 ] ;
						/* Top face vertices */
						p[ 4 ][ 0 ] = XX[ vertex_I   ][ vertex_J   ] ; p[ 4 ][ 1 ] = YY[ vertex_I   ][ vertex_J   ] ; p[ 4 ][ 2 ] = Z2[ vertex_I   ][ vertex_J   ] ;
						p[ 5 ][ 0 ] = XX[ vertex_I+1 ][ vertex_J   ] ; p[ 5 ][ 1 ] = YY[ vertex_I+1 ][ vertex_J   ] ; p[ 5 ][ 2 ] = Z2[ vertex_I+1 ][ vertex_J   ] ;
						p[ 6 ][ 0 ] = XX[ vertex_I+1 ][ vertex_J+1 ] ; p[ 6 ][ 1 ] = YY[ vertex_I+1 ][ vertex_J+1 ] ; p[ 6 ][ 2 ] = Z2[ vertex_I+1 ][ vertex_J+1 ] ;
						p[ 7 ][ 0 ] = XX[ vertex_I   ][ vertex_J+1 ] ; p[ 7 ][ 1 ] = YY[ vertex_I   ][ vertex_J+1 ] ; p[ 7 ][ 2 ] = Z2[ vertex_I   ][ vertex_J+1 ] ;

						for ( face_Index = 0; face_Index < 2; face_Index++ ){
							if ( face_Index == 0) {
								/* clockwise (seeing from inside) top vertices definition */
								A[ 0 ] = p[ 4 ][ 0 ] ;	A[ 1 ] = p[ 4 ][ 1 ] ;	A[ 2 ] = p[ 4 ][ 2 ] ;
								B[ 0 ] = p[ 5 ][ 0 ] ;	B[ 1 ] = p[ 5 ][ 1 ] ;	B[ 2 ] = p[ 5 ][ 2 ] ;
								C[ 0 ] = p[ 6 ][ 0 ] ;	C[ 1 ] = p[ 6 ][ 1 ] ;	C[ 2 ] = p[ 6 ][ 2 ] ;
								D[ 0 ] = p[ 7 ][ 0 ] ;	D[ 1 ] = p[ 7 ][ 1 ] ;	D[ 2 ] = p[ 7 ][ 2 ] ;
							}
							if ( face_Index == 1) {
								/* clockwise (seeing from inside) bottom vertices definition */
								A[ 0 ] = p[ 3 ][ 0 ] ;	A[ 1 ] = p[ 3 ][ 1 ] ;	A[ 2 ] = p[ 3 ][ 2 ] ;
								B[ 0 ] = p[ 2 ][ 0 ] ;	B[ 1 ] = p[ 2 ][ 1 ] ;	B[ 2 ] = p[ 2 ][ 2 ] ;
								C[ 0 ] = p[ 1 ][ 0 ] ;	C[ 1 ] = p[ 1 ][ 1 ] ;	C[ 2 ] = p[ 1 ][ 2 ] ;
								D[ 0 ] = p[ 0 ][ 0 ] ;	D[ 1 ] = p[ 0 ][ 1 ] ;	D[ 2 ] = p[ 0 ][ 2 ] ;
							}
							/* Translate vertices such that A -> A1=(0,0,0) */
						    B1[ 0 ] = B[ 0 ] - A[ 0 ] ;  B1[ 1 ] = B[ 1 ] - A[ 1 ] ;  B1[ 2 ] = B[ 2 ] - A[ 2 ] ;
						    C1[ 0 ] = C[ 0 ] - A[ 0 ] ;  C1[ 1 ] = C[ 1 ] - A[ 1 ] ;  C1[ 2 ] = C[ 2 ] - A[ 2 ] ;
						    D1[ 0 ] = D[ 0 ] - A[ 0 ] ;  D1[ 1 ] = D[ 1 ] - A[ 1 ] ;  D1[ 2 ] = D[ 2 ] - A[ 2 ] ;

						    /* Apply the translation to the point */
						    P1[ 0 ] = newCoord[ I_AXIS ] - A[ 0 ] ;
						    P1[ 1 ] = newCoord[ K_AXIS ] - A[ 1 ] ;
						    P1[ 2 ] = newCoord[ J_AXIS ] - A[ 2 ] ;

						    /* Transformation matrix */
						    X[ 0 ][ 0 ] = B1[ 0 ] ;  X[ 1 ][ 0 ] = B1[ 1 ] ;  X[ 2 ][ 0 ] = B1[ 2 ] ;
						    X[ 0 ][ 1 ] = D1[ 0 ] ;  X[ 1 ][ 1 ] = D1[ 1 ] ;  X[ 2 ][ 1 ] = D1[ 2 ] ;
						    X[ 0 ][ 2 ] = C1[ 0 ] ;  X[ 1 ][ 2 ] = C1[ 1 ] ;  X[ 2 ][ 2 ] = C1[ 2 ] ;

						    /* h = (B1 x D1).C1 */
						    E[ 0 ] = B1[ 1 ] * D1[ 2 ] - D1[ 1 ] * B1[ 2 ] ;
						    E[ 1 ] = B1[ 2 ] * D1[ 0 ] - D1[ 2 ] * B1[ 0 ] ;
						    E[ 2 ] = B1[ 0 ] * D1[ 1 ] - D1[ 0 ] * B1[ 1 ] ;

						    h = ( E[ 0 ] * C1[ 0 ]) + (E[ 1 ] * C1[ 1 ]) + (E[ 2 ] * C1[ 2 ] ) ;

						    if ( h == 0 ){
						    	/** Planar surface */
						    	if ( B1[ 0 ] == 0 ) {
						    		/* Along Oy */
						    		slp = D1[ 2 ] / D1[ 1 ] ;
						    		h = P1[ 1 ] * slp ;
						    		if ( face_Index == 0 && h >= P1[ 2 ] ) Inside[ face_Index ] = True ;
						    		else if ( face_Index == 1 && h <= P1[ 2 ] ) Inside[ face_Index ] = True ;
						    		else Inside[ face_Index ] = False ;
						    	}
						    	else if ( D1[ 0 ] == 0 ) {
						    		/* Along Ox */
						    		slp = B1[ 2 ] / B1[ 0 ] ;
									h = P1[ 0 ] * slp ;
									if ( face_Index == 0 && h >= P1[ 2 ]) Inside[ face_Index ] = True ;
									else if ( face_Index == 1 && h <= P1[ 2 ]) Inside[ face_Index ] = True ;
									else Inside[ face_Index ] = False ;
								}
								else {
						    		printf( "Problem with the planar surface for face number: %d\n" , face_Index ) ;
						    		abort();
						    	}
						    }
						    else {
						    	/* Transformation matrix */
						        H[ 0 ][ 0 ] = 1 ;  H[ 1 ][ 0 ] = 0 ;  H[ 2 ][ 0 ] = 0 ;
						        H[ 0 ][ 1 ] = 0 ;  H[ 1 ][ 1 ] = 1 ;  H[ 2 ][ 1 ] = 0 ;
						        H[ 0 ][ 2 ] = 1 ;  H[ 1 ][ 2 ] = 1 ;  H[ 2 ][ 2 ] = h ;

						        _SurfaceShape_invMatrix( X , Y ) ;

						        for ( i = 0; i < 3; i++ )
						            for ( j = 0; j < 3; j++ ) {
						            	sum = 0;
						            	for ( k = 0; k < 3; k++ )
						            		sum = sum + H[ i ][ k ] * Y[ k ][ j ];
						            	T[ i ][ j ] = sum;
						            }

						        /* Transformed point value */
						        P2[ 0 ] = T[ 0 ][ 0 ] * P1[ 0 ] + T[ 0 ][ 1 ] * P1[ 1 ] + T[ 0 ][ 2 ] * P1[ 2 ] ;
						        P2[ 1 ] = T[ 1 ][ 0 ] * P1[ 0 ] + T[ 1 ][ 1 ] * P1[ 1 ] + T[ 1 ][ 2 ] * P1[ 2 ] ;
						        P2[ 2 ] = T[ 2 ][ 0 ] * P1[ 0 ] + T[ 2 ][ 1 ] * P1[ 1 ] + T[ 2 ][ 2 ] * P1[ 2 ] ;

						        if ( P2[ 2 ] > h * P2[ 0 ] * P2[ 1 ] ) Inside[ face_Index ] = False ;
						        else Inside[ face_Index ] = True ;
							}

						}
					}
			}
		}

	for ( face_Index = 0; face_Index < 2; face_Index++ )
	    if ( !Inside[ face_Index ] ) return False ;

	return True;
}


double _SurfaceShape_CalculateVolume( void* polygon ) {
	assert( 0 );
	return 0.0;
}