#ifndef ShapeBase_H
#define ShapeBase_H

#include <iostream>
#include <ostream>
#include <fstream>
#include <stdio.h>
#include <string>
#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#include "Node.h"
#include "ReferenceShapeBase.h"
#include "GrowthFunctionBase.h"
#include "GrowthFunctionTypes.h"

class ShapeBase{
private:
    void ParentErrorMessage(std::string functionName);				///<Error message displayed when a virtual function is called through the ShapeBase(parent), while it should have been called through a child (eg. Prism). For functions taking in string inputs.
    bool ParentErrorMessage(std::string functionName, bool returnValue); 	///<Error message displayed when a virtual function is called through the ShapeBase(parent), while it should have been called through a child (eg. Prism). For functions taking in string and bool inputs, returning bool.
    double ParentErrorMessage(std::string functionName, double returnValue);	///<Error message displayed when a virtual function is called through the ShapeBase(parent), while it should have been called through a child (eg. Prism). For functions taking in string and double inputs, returning double.
    int ParentErrorMessage(std::string functionName, int returnValue);		///<Error message displayed when a virtual function is called through the ShapeBase(parent), while it should have been called through a child (eg. Prism). For functions taking in string and int inputs, returning int.
protected:
    int                         ShapeType;                      ///< The integer defining the type of the shape, Prisms shape type = 1;
    size_t                      nNodes;                         ///< The number of nodes of the element, it is based on ShapeBase#ShapeType
    size_t                      nDim;                           ///< The number of dimensions for the positions of each of the nodes of the element
    std::array<int,3>           IdentifierColour;				///< The unique identifier colour of the element, this is used for "picking" in the visual interface.
    std::array<double,3>        GrowthRate;                     ///< Growth rate recording for display purposes only. The recorded growth rate in x, y, and z  coordinates, does not record shear deformation induced in growth. Recorded in exponential form through time step, converted to rate per hour for display within the visual interface
    gsl_matrix*                 growthIncrement;				///< The matrix (3,3) representing the incremental growth in current time step. Reset to identity at the beginning of each time step, updated in growth functions, and utilised to update Fg.
    gsl_matrix*                 plasticDeformationIncrement;	///< The matrix (3,3) representing the incremental plastic deformation (treated as growth) in current time step. Set in plastic deformation calculation at each step, and utilised to update Fg.
    gsl_matrix*                 shapeChangeIncrement;           ///< The matrix (3,3) representing the incremental shape change in current time step. Reset to identity at the beginning of each time step, updated in shape change functions, and utilised to update Fg.
    double                      zRemodellingSoFar;				///< The z remodelling that have been applied to elemetn up to the current time step. This parameter is used to limit extreme thinning or elongation of elements.
    double                      columnarGrowthWeight;			///< The fraction defining how close to the columnar layer the element is. 1.0 for columnar layer, 0.0 for peripodial membrane elements, and scaled according to position in the elements surrounding the lumen.
    double                      peripodialGrowthWeight;         ///< The fraction defining how close to the peripodial membrane the element is. 0.0 for columnar layer, 1.0 for peripodial membrane elements, and scaled according to position in the elements surrounding the lumen.
    std::array<double,6>        ShapeChangeRate;				///< Shape change rate of the elements, only orthagonal shape changes are allowed (x, y, z). Shape changes will be scaled to conserve volume, thus three values will not be independent.
    bool                        rotatedGrowth;					///< The boolean stating if the element has rotated from the growth axis, hence the calculated growth requires further rotation to follow tissue axes.
    std::array<double,3>        relativePosInBoundingBox;		///< The relative position on x-y plane, within the bounding box of the tissue(x,y).
    std::array<double,3>        initialRelativePosInBoundingBox;///< The relative position on x-y plane, within the bounding box of the tissue(x,y) at the beginning of simulation. This is used when growth rates are pinned to the initial structure of the tissue.
    double                      initialRelativePositionInZ;     ///< The relative position on z-height of tissue, taken not in z direction but in tissue layers, 0 being on the apical surface and 1 being on the basal surface.
    size_t                      numberOfGaussPoints;            ///< The number of Gauss points used in numerical deforamtion calculation.
    std::array<std::array<double,3>,6>	gaussPoints;            ///< The array contianing all the Gauss points for element. Set up is for 6, can work for any number as loops are kept indexed up to numberOfGaussPoints.
    std::array<double,6>        gaussWeights;                   ///< The array for storing the weights of each Gauss point for element.
    std::vector<gsl_matrix*> 	ShapeFuncDerivatives;           ///< The array of matrices for shape function derivatives. The array stores a ShapeBase#nDim by ShapeBase#nNodes matrix for each gauss point (there are 3 Gauss points for prisms).
    std::vector<gsl_matrix*> 	ShapeFuncDerStacks;                    		///< The array of matrices of shape function derivatives in stacked format for ease of matrix operations. The array stores a (ShapeBase#nDim * ShapeBase#nDim) by (ShapeBase#nDim * ShapeBase#nNodes) matrix for each gauss point (there are 3 Gauss points for prisms).
    std::vector<gsl_matrix*> 	InvdXdes;                              		///< The array stores inverse of the matrix for derivatives of world coordinates with respect to barycentric coordinates (dX / de). The array stores an ShapeBase#nDim by ShapeBase#nDim  matrix for each gauss point (there are 3 Gauss points for prisms).
    std::array<double,6>     	detdXdes;                              		///< The array stores the determinants of the matrices for derivatives of world coordinates with respect to barycentric coordinates (dX / de). The array stores a double value for each gauss point (there are 3 Gauss points for prisms).
    std::vector<gsl_matrix*> 	Bmatrices;                             		///< The array stores the B matrix for the calculation of stiffness matrix, see for ShapeBase#calculateBTforNodalForces calculation. The array stores an ShapeBase#nNodes by (ShapeBase#nDim*ShapeBase#nNodes)  matrix for each Gauss point (there are 3 Gauss points for prisms).
    std::vector<gsl_matrix*> 	FeMatrices;					///< The array stores the elastic part of the deformation matrix. The array stores an ShapeBase#nDim by ShapeBase#nDim  matrix for each Gauss point (there are 6 Gauss points for prisms).
    std::vector<gsl_matrix*> 	invJShapeFuncDerStack;				///< The array stores the shape function derivatives multiplied by the inverse Jacobian stack, for each Gauss point. See ShapeBase#calculateBTforNodalForces for calculation.
    std::vector<gsl_matrix*> 	invJShapeFuncDerStackwithFe;			///< See ShapeBase#calculateInvJShFuncDerSWithFe for calculation.
    std::vector<gsl_matrix*> 	elasticStress;					///< The array of matrices for elastic stress of the element. The array stores a 6 by 6 matrix for each Gauss point (there are 6 Gauss points for prisms).
    std::vector<gsl_matrix*> 	viscousStress;					///< The array of matrices for internal viscous stress of the element. The array stores a 6 by 6 matrix for each Gauss point (there are 6 Gauss points for prisms).
    gsl_matrix*              	TriPointF;                             		///< The deformation matrix of the element resulting from iteration over all Gauss points. The dimensions of the matrix is ShapeBase#nDim by ShapeBase#nDim.
    gsl_matrix*              	ElementalElasticSystemForces;          		///< The matrix stores the elemental elastic forces. The dimensions of the matrix is ShapeBase#nNodes by ShapeBase#nDim.
    gsl_matrix*          	ElementalInternalViscousSystemForces;  		///< The matrix stores the elemental internal viscous forces. The dimensions of the matrix is ShapeBase#nNodes by ShapeBase#nDim.
    std::array<double,6> 	detFs;                              		///< The array stores the determinant of the deformation matrix for each Gauss point.
    double 			ZProjectedBasalArea;				///< The z-projected area of the basal surface of the element.
    double 			ZProjectedApicalArea;			    	///< The z-projected area of the apical surface of the element.
    double 			BasalArea;					///< The area of the basal surface of the element.
    double 			ApicalArea;					///< The area of the apical surface of the element.
    double			exposedLateralAreaApicalSide;			///< The area of the element on a linker position, and has lateral sides exposed to outside of the tissue, on the apical side, therefore should feel external viscosity.
    double			exposedLateralAreaBasalSide;			///< The area of the element on a linker position, and has lateral sides exposed to outside of the tissue, on the basal side, therefore should feel external viscosity.

    bool 			elementHasExposedApicalSurface;			///< The boolean stating if the element has any apical surface exposed to the environment
    bool 			elementHasExposedBasalSurface;			///< The boolean stating if the element has any basal surface exposed to the environment
    int 			exposedApicalSurfaceNodeIds[3];			///< The int array of size 3, listing the node IDs of element that form the exposed apical surface. The IDs are the node IDs on the element (0-5 for prism), not the actual Node#Id.
    int 			exposedBasalSurfaceNodeIds[3];			///< The int array of size 3, listing the node IDs of element that form the exposed basal surface. The IDs are the node IDs on the element (0-5 for prism), not the actual Node#Id.
    int 			exposedLateralAreaApicalSideNodeIds[4];		///< The int array of size 4, listing the node IDs of element that form the lateral surface exposed apically. The IDs are the node IDs on the element (0-5 for prism), not the actual Node::Id.
    int 			exposedLateralAreaBasalSideNodeIds[4];		///< The int array of size 4, listing the node IDs of element that form the lateral surface exposed basally. The IDs are the node IDs on the element (0-5 for prism), not the actual Node::Id.
    size_t 			nLateralSurfaceAreaNodeNumber;			///< Number of nodes that form the lateral surfaces for the element.
    size_t 			nSurfaceAreaNodeNumber;				///< Number of nodes that form the apical/basal surfaces for the element.

    double 			stiffnessPerturbationRateInSec;         	///< The rate at which the stiffness of the element will be perturbed, used with the model inputs from "Stiffness_Perturbation:" header in model input file
    double 			minimumValueOfStiffnessMultiplier;		///< The lower bound of stiffness modification multiplier, exists to prevent elements reaching unintended zero or negative stiffness values.
    double 			maximumValueOfStiffnessMultiplier;		///< The upper bound of stiffness modification multiplier, exists to prevent elements reaching unrealistic hard stiffness values.
    double 			mutationGrowthRatePerSec;			///< The growth rate set by a mutant clone covering this element.
    double 			mutationGrowthFold;				///< The rate of fold change in growth rate set by a mutant clone covering this element.
    void 			setShapeType(std::string TypeName);		///< The function sets the type of the shape.
    void 			readNodeIds(const std::vector<int>& inpNodeIds);///< The function sets the Node#Id array that constructs the shape.
    void 			setPositionMatrix(const std::vector<std::unique_ptr<Node>>& Nodes);	///< The function sets the ShapeBase#Positions matrix to define the locations of each constructing node.
    void 			setTissuePlacement(const std::vector<std::unique_ptr<Node>>& Nodes);	///< The function sets the placement of the element within the tissue
    void 			setTissueType(const std::vector<std::unique_ptr<Node>>& Nodes);		///< The function sets the tissue type of the element
    void 			setReferencePositionMatrix();						///< The function sets the RefereneceShapeBase#Positions matrix to define the reference positions of the element.
    void 			setIdentificationColour();						///< The function sets the unique ShapeBase#IdentifierColour colour for the element, which is used in element picking from the user interface.
    void 			rotateReferenceElementByRotationMatrix(std::array<double,9> rotMat);	///< The function rotates the reference of the element (ShapeBase#ReferenceShape) by input rotation matrix, provided as a double pointer of 9 doubles.
    bool 			InvertMatrix(boost::numeric::ublas::matrix<double>& input, boost::numeric::ublas::matrix<double>& inverse); ///< The function takes the first input matrix, and writes the inverse on the second input. False is returned if the matrix is not inverted. Input format is ublas matrices (slow).
    bool 			InvertMatrix(gsl_matrix* input, gsl_matrix* inverse); 			///< The function takes the first input matrix, and writes the inverse on the second input. False is returned if the matrix is not inverted. Input format is gsl matrices (fast).

    void 			updateNodeIdsFromSave(std::ifstream& file);				///< The function reads the ShapeBase#NodeIds of the current shape from save file provided as input.
    void 			updateReferencePositionMatrixFromSave(std::ifstream& file); 		///< The function reads and updates the ShapeBase#ReferenceShape positions (ReferenceShapeBase#Positions) of the current shape from save file provided as input.
    virtual void 		calculateReferenceVolume(){ParentErrorMessage("calculateReferenceVolume");}  ///<Virtual function of the ShapeBase class to calculate volume of the ShapeBase#ReferenceShape

    bool                calculateGrowthStrainsRotMat(double* v);				///< The function calculates the rotation matrix to apply on growth strains to align growth with the current x axis of the tissue.
    void                calculateForces3D(const std::vector<std::unique_ptr<Node>>& Nodes,  gsl_matrix* displacementPerDt); ///< The function calculates the viscous and elastic forces generated by the element.
    gsl_matrix* 		calculateEForNodalForcesKirshoff(gsl_matrix* C);			///< This function calculates the green strains for a Kirshoff material model.
    gsl_matrix* 		calculateCauchyGreenDeformationTensor(gsl_matrix* Fe);			///< This function calculates the Caucy-Green deformation tensor, from the elastic part of the deformation gradient
    gsl_matrix* 		calculateSForNodalForcesKirshoff(gsl_matrix* E);			///< This function calculates the Secons order Piola-Kirshoff stress tensor for Kirshoff material model.
    gsl_matrix* 		calculateSForNodalForcesNeoHookean(gsl_matrix* invC, double lnJ);	///< This function calculates the Secons order Piola-Kirshoff stress tensor for Neo-Hookean material model.v
    void 			updateLagrangianElasticityTensorNeoHookean(gsl_matrix* invC,double lnJ, int pointNo);	///< This function calcualtes the Lagrangian elasticity tensor for Neo-Hookean material model.
    gsl_matrix* 		calculateCompactStressForNodalForces(double detFe,gsl_matrix* Fe, gsl_matrix* S, gsl_matrix *Stress); ///< This function calculates elemental stress in Voigt notation.
    gsl_matrix* 		calculateInverseJacobianStackForNodalForces(gsl_matrix* Jacobian);	///< This function calculates the stack matrix of inverse Jacobians, used to calculate the nodal forces.
    gsl_matrix* 		calculateBTforNodalForces(gsl_matrix* InvJacobianStack, gsl_matrix* ShapeFuncDerStack, gsl_matrix *B, gsl_matrix* invJShFuncDerS); ///< This function calculates the B matrix, to calculate the nodal force.
    void			calculateInvJShFuncDerSWithFe(gsl_matrix * currFe, gsl_matrix * InvDXde, gsl_matrix* ShapeFuncDerStack, gsl_matrix *invJShFuncDerSWithF); ///< This function calculates the collated matrix from inverse jaconians and shape function derivatives.
    gsl_matrix* 		calculateVelocityGradientTensor(gsl_matrix* B, gsl_matrix* displacementPerDt);  ///< This function calculates the velocity gradient tensor.
    gsl_matrix* 		constructElementalDisplacementMatrix(gsl_matrix* displacement); 		///< This function will assemble elemental node displacement matrix from the input displacement matrix for the whole system.
    gsl_matrix* 		calculateRateOfDeformationTensor(gsl_matrix* l);				///< This function will calculate rate of deformation tensor from velocity gradient tensor
    void 			calculateViscousStress(gsl_matrix* d, gsl_matrix* viscousStress);		///< This function will calculate internal viscous stress of the element from rate of deformation matrix.
    void 			calculateViscousForces(gsl_matrix*  gv, gsl_matrix*  BTdetFdetdXde, gsl_matrix* viscousStress); ///< This function  will calculate the elemental viscous forces from viscous stress.

    void    			consturctBaTBb(gsl_matrix* B, gsl_matrix* BaT, gsl_matrix* Bb, int a, int b);	///< This function constructs nodal subrange of B matrix for node couple a & b.
    void   			calculateElasticKIntegral1(gsl_matrix* currElementalK,int pointNo);		///< This function calcultes the first part of the integral for the stiffness matirx, the elastic part of the system Jacobian.
    void			calculateElasticKIntegral2(gsl_matrix* currElementalK,int pointNo);             ///< This function calcultes the second part of the integral for the stiffness matirx, the elastic part of the system Jacobian.
    void			calculateViscousKIntegral1(gsl_matrix* currElementalK, gsl_matrix* paranthesisTermForKv1, int pointNo);	///< This function calcultes the first part of the integral for the internal viscous part of the system Jacobian.
    void			calculateViscousKIntegral2(gsl_matrix* currElementalK,int pointNo);             ///< This function calcultes the second part of the integral for the viscous part of the system Jacobian.
    void			calculateVelocityGradient( gsl_matrix* velocityGradient, gsl_matrix* displacementPerDt, int pointNo);	///< This function calculates the velocity gradient.
    void			calculateOuterProduct(gsl_matrix* a, gsl_matrix* b, gsl_matrix* outerProduct);	///< Calculates the outer product, maths helper function.
    gsl_matrix* 		calculateSymmetricisedTensorProduct(gsl_matrix* a, gsl_matrix* b);          ///< Calculates the symetricised tensor product, maths helper function.

    bool 			disassembleRotationMatrixForZ(gsl_matrix* rotMat);			///< This function extracts the z rotation from a rotation matrix.
    bool 			calculate3DRotMatFromF(gsl_matrix* rotMat);					///< This function dissects the deformation gradient of the element into the rigid body rotation and deformation.

    gsl_matrix* 		D;                                                      ///< elasticity tensor for Kirshoff material
    gsl_matrix* 		CoeffMat;                                               ///< The coefficient matrix relating the shape function derivative stack to the Voigt notation of elemental stress nad strain.
    //need to construct double array of size: D81[nGaussPoints][3][3][3][3];
    std::vector<std::array<std::array<std::array<std::array<double,3>,3>,3>,3>> D81;				///<Lagrangian elasticity tensor, vector for the number of Gauss points in simulaiton.
    double 			E;                                                          ///< Young's modulus of the element.
    double			v;                                                          ///< Poisson's ratio of the element.
    double 			internalViscosity;                                          ///< Current internal viscosity of the element.
    double 			originalInternalViscosity;                                  ///< The internal viscosity of the element at the beginning of the simulation, prior to physical property perturbations.
    double 			lambda;                                                     ///< Lame's second parameter, driven from Young's modulus and Poisson's ratio of the element
    double			mu;                                                         ///< Sheer modulus of the element.
    gsl_matrix* 	InvFg;                                                      ///< Inverse of growth matrix
    gsl_matrix* 	Fsc;                                                        ///< Shape change matrix
    gsl_matrix* 	InvFsc;                                                     ///< Inverse of shape change matrix
    gsl_matrix* 	TriPointKe;                                                 ///< Current elastic part of the Jacobian (stiffness matrix) of the system, averaged over all Gauss Points.
    gsl_matrix* 	TriPointKv;                                                 ///< Current viscous part of the Jacobian of the system, averaged over all Gauss Points.
public:
    double 			stiffnessMultiplier;                                        ///< Current stiffness multiplier of the element, initially 1.0, modulated by stiffness perturbations						///< The double for the multiplier that will define Young's modulus stress stiffening.
    gsl_matrix*		remodellingPlaneRotationMatrix;                             ///< The rotation matrix converting the xyz coordinate system to the plane of remodelling for the lateral elements.
    gsl_matrix* 	Fg;                                                         ///< Growth deformation gradient
    int 			Id;                                                         ///< The unique ID of the element, without remodelling, equal to its indes on the Simulation#Elements vector.
    int				ShapeDim;                                                   ///< The dimension of the shape in workd coordiantes (2D vs 3D).
    std::vector<int> NodeIds;                                                   ///< The vector storing the unique IDs (Node#Id) of nodes constructing this element. Their order is consistent for a given shape type.
    virtual ~ShapeBase(){                                                       /// The ShapeBase destructor. This destructor should not be called uner healthy conditions.
        //while deleting a ShapeBase* that happens to point a child, this destructor will be called after the child destructor
    }
    std::vector<std::array<double,3>> 	Positions;								///< The vector storing the positions of the nodes constructing the element.
    std::unique_ptr<ReferenceShapeBase> ReferenceShape;                         ///< The pointer to the reference shape object that defines th reference shape of this element.
    gsl_matrix* 	Strain;                                                     ///< The gsl_matrix pointer, storing the address of the current strains on the element.

    bool 			isFlipped;                                                  ///< Boolean stating if the element is flipped. The simulation will be stopped if there are flipped elements.
    bool 			IsChangingShape;
    //bool			willBeRefined;
    int 			tissuePlacement;                                            ///< 1 -> apical, 0 -> basal, 2->middle, 3 -> lateral
    int 			tissueType;                                                 ///< The tissue type is 000 for columnar layer, 1 for peripodial membrane, and 2 for linker zone
    bool			spansWholeTissue;                                           ///< Boolean staing is the element spans the whole tissue. This is used to identify mid-layer tagged tissues (tissuePlacement = 2), that should still have apical abd basal responses
    int 			compartmentType;                                            ///< integer identifying the compartment of the tissue in DV axis, 0 pouch, 1 hinge, 2 notum
    double 			compartmentIdentityFraction;                                ///< The weight defining the constibution of each compartment to the physical identity of this element.
    bool			isECMMimicing;                                              ///< Boolean stating if the element is an ECM element.
    bool			isECMMimimcingAtCircumference;                              ///< Boolean stating if the element is an ECM element at the circumference of the tissue.
    bool			atBasalBorderOfECM;                                         ///< Boolean stating if the element is at the basal border of the cellular layer, linking to ECM elements.
    bool			isActinMimicing;                                            ///< Boolean stating if the element is forming the actin dense layer on the apical surface.
    bool			atApicalBorderOfActin;                                      ///< Boolean stating if the element is at the apical border of the soft cellular layer, linking to actin dense layer.
    bool			IsAblated;                                                  ///< Boolean stating if the element is laser ablated, therefore dead.
    bool			atSymetricityBoundary;                                      ///< Boolean stating if the element is at the symmetricity boundary.
    //bool			IsClippedInDisplay;
    //bool 			IsXSymmetricClippedInDisplay;
    //bool			IsYSymmetricClippedInDisplay;
    double 			CurrShapeChangeToAdd[3];                                    ///< The current shape change to be applied to the element, in form of 3D rates.
    double  		GrownVolume;                                                ///< Current volume of the element after growth.
    double  		VolumePerNode;                                              ///< Volume per node of the element.
    bool 			capElement;                                                 ///< Boolean stating if the element is capped at its remodelling due to restrictions in z remodelling (avoiding too thin ot too thick layers due to numerical error).
    std::vector<int> elementsIdsOnSameColumn;                                   ///< The vector storing the unique element IDs of each element that is on the same columnar region of the tissue, i.e. the elements share apical/basal surfaces.
    int 			basalNeigElementId;                                         ///< This is recorded only for apical nodes of the columnar layer. If not recorded, id is -1.
    bool 			insideEllipseBand;                                          ///< Boolean stating if the element is marked by any identifier bands for physical perturbation.
    int 			coveringEllipseBandId;                                      ///< The unique ID of the covering perturbation band.

    double 			emergentShapeLongAxis[2];                                   ///< The long axis of the emergent shape. This is necessary for analysis of emergent growth orientations.
    double 			emergentShapeShortAxis[2];                                  ///< The short axis of the emergent shape. This is necessary for analysis of emergent growth orientations.

    double			plasticDeformationHalfLifeMultiplier;                       ///< The multiplier to modify the remodelling half-life upon physical property perturbation.
    bool 			isMutated;                                                  ///< Boolean stating if the element is mutated.

    bool 			thereIsGrowthRedistribution;                                ///< Boolean stating if there is redistribution of growth among mesh elements.
    bool 			growthRedistributionShrinksElement;                         ///< Boolean stating if the growth distribution is taking material out of thes element to redistribute ot others.
    double 			growthRedistributionScale;                                  ///< The extent of the redistribution of volume.
    bool			RotatedElement;                                             ///< The boolean stating if the elemetn has rigid body rotation.
    gsl_matrix* 	GrowthStrainsRotMat;                                        ///< The rotation matrix needed to correct for the rigid body rotations of the element.

    std::array<double,3> 	apicalNormalCurrentShape;							///< The apical normal of the current shape.
    int                         getId();                                        ///< The function returns the unique ID of the element.
    std::string                 getName();                                      ///< The function returns the name of the element.
    int                         getShapeType();									///< The function returns the shape type of the element.
    size_t                      getNodeNumber();								///< The function returns the number of nodes o the element.
    const std::vector<int>& 	getNodeIds();									///< The function returns the vector of node IDs.
    int                         getNodeId(int i);								///< The function returns the input i^{th} node's ID.
    size_t                      getDim();                                       ///< The function returns the dimensions of the node, ShapeBase#Dim.
    std::array<int,3>           getIdentifierColour();							///< The unique [r,g,b] identifier colour of the element, utilised in picking in the user interface.
    std::array<double,3>        getCentre();									///< This function returns the centre of the element in world spave.
    double                      getPeripodialness();							///< This function returns the relative influence of the peripodial physical characteristics to this element.
    double                      getColumnarness();								///< This function returns the relative influence of the columnar  physical characteristics ot this element.
    void                        getRelativePositionInTissueInGridIndex(int nGridX, int nGridY, int& IndexX, int& IndexY, double& FracX, double& FracY); 	///< Provides the relative position within the bounding box of the tissue, and calculates which point on the growth maps should be read.
    void                        getInitialRelativePositionInTissueInGridIndex(int nGridX, int nGridY, int& IndexX, int& IndexY, double& FracX, double& FracY); 	///< The relative positions of the iitial configuration of the element within the bounding box of the tissue.
    double                      getStiffnessMultiplier();							///< This function returns the current stiffness multiplier as a result of perturbations to physical properties.
    double                      getCurrentVolume();								///< This function returns the current volume of the element.
    double                      getElementalElasticForce(int nodeIndex, int dimIndex);				///< This function returns the calculated elastic force for the node nodeIndex, in coordinate dimIndex.
    void                        setElementalElasticForce(int nodeIndex, int dimIndex, double value);		///< This functions sets the calculated elastic force for the node nodeIndex, dimension dimIndex, to the input value.

    gsl_matrix*             getCurrentFe();									///< This function returns the current elastic part of the deformation gradient.
    double                  getApicalArea();								///< This function returns the current apical area of the element.
    void                    relaxElasticForces();								///< This function relaxes all teh accumulated elastic forces in the system.
    bool                    isGrowthRateApplicable(int sourceTissue, double& weight, double zmin, double zmax);	///< The function checks if the element if affected by the current growth functions.
    void                    updateGrowthWillBeScaledDueToApikobasalRedistribution(bool thisFunctionShrinksApical, std::vector<int>& ellipseBandIdsForGrowthRedistribution); ///< This function decide if the growth will be redistirbuted in the height of the tissue.
    void                    scaleGrowthForZRedistribution( double& x, double& y, double& z);			///< This function will modify the incremental growth deformation gradient of the element to reflect the volume redistribution in the height of the tissue.
    void                    calculateFgFromRates(double dt, double x, double y, double z, gsl_matrix* rotMat, gsl_matrix* increment, int sourceTissue, double zMin, double zMax);	///< This fucntion will calculate the incremental growth deformation gradient change for the current time step, from input growth rates
    void                    calculateFgFromGridCorners(int gridGrowthsInterpolationType, double dt, GrowthFunctionBase* currGF, gsl_matrix* increment, int sourceTissue, int IndexX, int IndexY, double FracX, double dFracY); ///< This fucntion will calculate the incremental growth deformation gradient change for the current time step by reading it from the grid, and interpolating on 4 corners.
    gsl_matrix*             getGrowthIncrement();								///< This function will return the current growth deformation gradient increment
    void                    updateGrowthIncrement(gsl_matrix* columnar, gsl_matrix* peripodial);		///< This function will update the elemental growth deformation gradient from the current growth deformation gradient increment.
    void                    updateGrowthByMutation(double dt);						///< This function will update the growth growth deformaton gradient increment of the element due to a mutation.
    void                    scaleGrowthIncrement(double multiuplier);					///< This function will scale the growth growth deformaton gradient increment by input double.
    void                    calculateShapeChangeIncrementFromRates(double dt, double rx, double ry, double rz, gsl_matrix* increment); ///< This function will calculate the current shape change deformation gradient increment due to elemental active shape change, from input rates.
    void                    updateShapeChangeIncrement(gsl_matrix* columnarShapeChangeIncrement);		///< This function updates the growth increment of the element with the current shape change increment
    void                    calculateRelativePosInBoundingBox(double boundingBoxXMin, double boundingBoxYMin, double boundingBoxLength, double boundingBoxWidth); ///< This function will calculate the relative positions of the element in the xy-plane bounding box of the tissue.
    void                    mutateElement(double growthFold, double growthRatePerHour);			///< This function will set the element as a mutant, the mutant growth rates will be set.
    void                    updateReferencePositionMatrixFromInput(double** input);				///< This function will update the reference position matrix. Not used under healthy, continuous simulations, to preserve continuity of the mesh.
    void                    displayRelativePosInBoundingBox();						///< Helper function to display the relative position of the element in the bounding box of the tissue.
    std::array<double,2>	getRelativePosInBoundingBox();							///< This function will return the relative position of the element in the xy bounding box of the tissue
    void 			setRelativePosInBoundingBox(double x, double y);				///< This functoin will set the relative position of the element in the bounding box of the tissue to the input coordinates.
    void			setInitialRelativePosInBoundingBox();						///< This function sets the initial relative position in bounding box of the tissue to current reference position.
    void 			setInitialZPosition(double zMax, double TissueHeight);				///< This fucntion sets the initial relative z position of the tissue in tissue height.
    std::array<double,2>	getInitialRelativePosInBoundingBox();						///< This function will return the initial relative position of the element in the xy bounding box of the tissue
    void 			convertRelativePosToGridIndex(std::array<double,2> relpos, int& indexX, int &indexY, double &fracX, double &fracY, int nGridX, int nGridY); ///< This function will convert the relative position of the tissue in xy plane bounding box to growth map grid indices.
    void 			getStrain(int type, float &StrainMag);						///< This function will return the selected strain component of the element.
    void 			getNodeBasedPysProp(int type, int NodeNo, const std::vector<std::unique_ptr<Node>>& Nodes, float& PysPropMag); ///< This function will return the selected physical properties of the element on a nodal basis.
    void 			getPysProp(int type, float &PysPropMag, double dt);				///< This function will return the selected physical properties of the element.
    double			getInternalViscosity();								///< This function will return the internal viscosity of the element.
    double			getOriginalInternalViscosity();							///<  This function will return the internal viscosity of the element prior to any perturbations.
    //void   			updateInternalViscosityTest();
    double 			getYoungModulus();								///< This function will return the Young's modulus of the element
    double 			getPoissonRatio();								///< This function will return the Poissons's ratio of the element
    const std::array<double,3>& getGrowthRate();								///< This function will return the current growth rate of the element.
    const std::array<double,6>& getShapeChangeRate();								///< This function will return the current shape change rate of the element.
    const std::vector<std::array<double,3>>& getReferencePos();							///< This function will return the reference positions of the element.
    void    			getPos(gsl_matrix* Pos);							///< This function will write the position of the element into input matrix.
    gsl_matrix* 		getFg();									///< This function will return the growth component of the deformation gradient.
    gsl_matrix* 		getInvFg();									///< This function calculates the inverse of the growth deformation gradient matrix.
    gsl_matrix* 		getFsc();									///< This function will return the shape change component of the deformation gradient.
    gsl_matrix* 		getInvFsc();									///< This function calculates the inverse of the shape change deformation gradient matrix.
    gsl_matrix* 		getFe();									///< This function will return the elastic component of the deformation gradient.
    double 			getZRemodellingSoFar();								///< This function will return the z remodelling applied to the element so far, to cap the z remodelling.
    void 			setZRemodellingSoFar(double zRemodellingSoFar); 				///< This function will set the z remodelling applied to the element so far to input value. This is needed during saved input reading.
    void 			displayName();									///< Helper function, display the name of the element
    void			displayNodeIds();								///< Helper function, display the Ids of the nodes of the element
    void 			displayPositions();								///< Helper function, display the nodal positions of the element
    void 			displayReferencePositions();							///< Helper function, display the nodal positions of the reference element
    void 			displayIdentifierColour();							///< Helper function, display the unique identifier colour, for picking in user interface.
    void  		  	setFg(gsl_matrix* currFg);							///< This function sets the current grwoth deformation gradient matrix equal to input matrix
    void 			setGrowthWeightsViaTissuePlacement (double periWeight);				///< This function sets the weight for growth rate scaling depending on tissue type
    void 			setYoungsModulus(double E);							///< This function sets the Young's modulus of the shape to inout double.
    virtual void 		setElasticProperties(double /*EApical*/,double /*EBasal*/, double /*EMid*/, double /*EECM*/, double /*v*/){ParentErrorMessage("setElasticProperties");} ///< This is the parent virtual function for setting up the elasticity properties of the shape depending on its tissue type placement.
    virtual void 		checkEdgeLenghtsForBinding(std::vector<int>& /*masterIds*/, std::vector<int>& /*slaveIds*/){ParentErrorMessage("checkEdgeLenghtsForBinding");} ///< The virt
    void 			setViscosity(double viscosityApical,double viscosityBasal, double viscosityMid);///< This function sets the viscosity of the element depending on its placement in the tissue.
    void 			setViscosity(double viscosityApical,double viscosityBasal);			///< This function sets the viscosity of the element depending on its placement in the tissue.
    void 			setViscosity(double viscosity);							///< This function sets the viscosity of the element depending on its placement in the tissue.
    double			calculateEmergentShapeOrientation();						///< This fucntion calculates the orientation of the emergent shape of an element in the xy plane of the tissue.
    bool 			isActinStiffnessChangeAppliedToElement(bool ThereIsWholeTissueStiffnessPerturbation, bool ThereIsApicalStiffnessPerturbation, bool ThereIsBasalStiffnessPerturbation, bool ThereIsBasolateralWithApicalRelaxationStiffnessPerturbation, bool ThereIsBasolateralStiffnessPerturbation, std::vector <int> &stiffnessPerturbationEllipseBandIds, int numberOfStiffnessPerturbationAppliesEllipseBands ); ///< This function decides if the actin stiffness perturbation is applied to this element.
    bool 			isECMChangeAppliedToElement(bool changeApicalECM, bool changeBasalECM, std::vector<int> &ECMChangeEllipseBandIds, int numberOfECMChangeEllipseBands);				///< This function decides if the ECM perturbation is applied to this element
    bool 			isShapeChangeAppliedToElement(std::vector<int> &ellipseBandIds, bool applyBasalECM, bool applyToLateralECM, bool applyApically, bool applyBasally, bool applyMidLayer );	///< This function decides if the shape change perturbation is applied to this element
    void 			calculateStiffnessPerturbationRate(bool ThereIsBasolateralWithApicalRelaxationStiffnessPerturbation, double stiffnessPerturbationBeginTimeInSec, double stiffnessPerturbationEndTimeInSec, double stiffnessChangedToFractionOfOriginal); ///< This function will calciulate the stiffness perturbation rate.
    void 			updateStiffnessMultiplier(double dt); ///< The function will update the actin multiplier as a result of stiffness perturbations.
    virtual std::array<double,3>  calculateBasalNormal(){ParentErrorMessage("calculateBasalNormal"); std::array<double,3> dummy = {0.0, 0.0, 0.0}; return dummy;} 	///< The virtual function of the parent for basal normal calculation. The value is dependent on node topology of the element and defined for eac individual child class.
    virtual void 		calculateApicalNormalCurrentShape(){ParentErrorMessage("calculateApicalNormal");}							///< The virtual function of the parent for apical normal calculation. The value is dependent on node topology of the element and defined for eac individual child class.
    void 			calculateCurrentGrowthIncrement(gsl_matrix* resultingGrowthIncrement, double dt, double growthx, double growthy, double growthz, gsl_matrix* ShearAngleRotationMatrix); ///< The function calculates the current growth increment from the input of growth rate and the orientation rotation matrix.
    void 			updateShapeChangeRate(double x, double y, double z, double xy, double yz, double xz);
    virtual void 		calculateElementShapeFunctionDerivatives(){ParentErrorMessage("calculateElementShapeFunctionDerivatives");}	///< The virtual function of the parent for calculation of shape function derivatives. This is topology dependent and implemented in each child shape type.
    virtual void 		calculateCurrNodalForces(gsl_matrix */*gslcurrge*/, gsl_matrix */*gslcurrgv*/, gsl_matrix */*gslcurrF*/, gsl_matrix* /*displacementPerDt*/, int /*pointNo*/){ParentErrorMessage("calculateCurrNodalForces");} ///< The virtual function to calculate nodal force. This is topology dependent and implemented in each child shape type.
    virtual void 		calculateCurrTriPointFForRotation(gsl_matrix */*currF*/,int /*pointNo*/){ParentErrorMessage("calculateCurrTriPointFForRotation");}
    virtual void 		calculateApicalArea(){ParentErrorMessage("calculateApicalArea");}                           ///< The virtual function to calculate apical area of element. This is topology dependent and implemented in each child shape type.
    virtual void 		calculateBasalArea(){ParentErrorMessage("calculateBasalArea");}                             ///< The virtual function to calculate basal area of element. This is topology dependent and implemented in each child shape type.
    double 			calculateCurrentGrownAndEmergentVolumes();                                                      ///< This is the function to calculate hte current ideal volume of the element and its current apparent volume.
    virtual void 		updateElasticProperties(){ParentErrorMessage("updateElasticProperties");}                   ///< This functions updates elastic propertiesand their dependent tensors upon alteration of a physical property.
    void 			writeInternalForcesTogeAndgv(gsl_matrix* ge, gsl_matrix* gvInternal, std::vector<std::array<double,3>>& SystemForces, const std::vector<std::unique_ptr<Node>>& Nodes); ///< This function writes the elemental elastic and viscous forces to the system scale force vector.
    void 			calculateForces(const std::vector<std::unique_ptr<Node>>& Nodes, gsl_matrix* displacementPerDt);
    void 			updatePositions(const std::vector<std::unique_ptr<Node>>& Nodes);                               ///< This function updates the position array of the element from the updated nodal posiitons.
    void 			setGrowthRate(double dt, double rx, double ry, double rz);                                      ///< This function sets the growth of the element from the tome step and the input rates
    void 			setGrowthRateViaInputTimeMultipliedMagnitude(double x, double y, double z);                     ///< This function sets the growht rate to pre-calculated rates given as input.
    void 			updateGrowthIncrementFromRate();                                                                ///< This function fills th egrowht increment matirx from the current growth rate matrix
    double  		calculateVolumeForInputShapeStructure(std::vector<std::array<double,3>> shapePositions, size_t nTriangularFaces, std::vector<std::array<int,3>>  triangularFaces, std::array<double,3> midPoint ); ///< This function calculates the volume of the shape, it is generalised such that the shape is defined as an array of triengles forming a convex hull.
    void 			calculatePrincipalStrains3D(double& e1, double &e2,  double &e3, gsl_matrix* eigenVec);         ///< This function calculates the principal components of the strains through eigen values and eigen vectors in 3D.
    void 			calculatePrincipalStrains2D(double& e1, double &e2,  double &e3, gsl_matrix* eigenVec);         ///< This function calculates the principal components of the strains through eigen values and eigen vectors in 2D.
    //void 			calculatePrincipalStrainAxesOnXYPlane(double& e1, double &e2, double& tet);			///< This function princial
   // bool			checkIfXYPlaneStrainAboveThreshold(double thres);
   // bool 			calculateIfInsideActiveStripe(double initialPoint,double endPoint, double stripeSize1, double stripeSize2);
    void 			setShapeChangeRate(double x, double y, double z, double xy, double yz, double xz);                  ///< This function sets the shape change rate to pre-calculated rates given as input.
    void  			setShapeChangeInrementToIdentity();                                                                 ///< This function sets the shape change deformation gradient increment to identity
    void 			updateElementVolumesAndTissuePlacementsForSave(const std::vector<std::unique_ptr<Node>>& Nodes);    ///< This function calculates the reference volume, tissue placement and tissue type from nodal information
    bool 			readNodeIdData(std::ifstream& file);                                                                ///< This function reads in the node Ids for the element from save file
    bool			readReferencePositionData(std::ifstream& file);                                                     ///< This function reads the reference element positions from save file

    bool 			areanyOfMyNodesAtCircumference(const std::vector<std::unique_ptr<Node> > &Nodes);                   ///< This function check if the element owns any node at tissue circumference

    virtual void 		 checkHealth(){ParentErrorMessage("checkHealth");}                                              ///< The virtual function in parent to check if element is flipped, implemented for each child as it is node topology dependent.
    void    			writeKelasticToMainKatrix(gsl_matrix* K);                                                       ///< This function writes the elemental elastic component of the Jacobian to system Jacobian.
    void    			writeKviscousToMainKatrix(gsl_matrix* K);                                                       ///< This function writes the elemental viscous component of the Jacobian to system Jacobian.
    void    			calculateImplicitKElastic();                                                                    ///< This function calculates the elemental elastic component of the Jacobian for implicit NR itaration.
    void 			calculateImplicitKViscous(gsl_matrix* displacementPerDt, double dt);                                ///< This function calculates the elemental viscous component of the Jacobian for implicit NR itaration.
    void			calculateForceFromStress(int nodeId, gsl_matrix* Externalstress, gsl_matrix* ExternalNodalForces);  ///< This function calculates the elemental nodal forces from an input external stress matrix

    void 			updateShapeFromSave(std::ifstream& file);                                           ///< This function updates the element geometric poroerties from save file.
    void 			displayMatrix(boost::numeric::ublas::matrix<double>& mat, std::string matname);     ///< Helper function, displays the input blas (double) matrix with the input name.
    void 			displayMatrix(boost::numeric::ublas::matrix<int>& mat, std::string matname);        ///< Helper function, displays the input blas (int) matrix with the input name.
    void 			displayMatrix(boost::numeric::ublas::vector<double>& vec, std::string matname);     ///< Helper function, displays the input blas (double) vector with the input name.
    void 			displayMatrix(gsl_matrix* mat, std::string matname);                                ///< Helper function, displays the input gsl matrix with the input name.
    void 			displayMatrix(gsl_vector* mat, std::string matname);                                ///< Helper function, displays the input gsl vector with the input name.
    void 			createMatrixCopy(gsl_matrix *dest, gsl_matrix* src);                                ///< Helper function, creates a copy of the gsl matrix on new memory locaiton.
    double  		calculateMagnitudeVector3D(std::array<double,3> v);                                 ///< Helper algebraic function, calculates norm of the vector defined in the array<double,3>;
    void			normaliseVector3D(gsl_vector* v);                                                   ///< Helper algebraic function, normalises the input gsl vector (the input vector is modified)
    double			normaliseVector3D(std::array<double,3>& v);                                         ///< Helper algebraic function, normalises the input array<double,3> (the input vector is modified)
    double			getNormVector3D(gsl_vector* v);                                                     ///< Helper algebraic function, calculates norm of the vector defined in the gsl vector, the vector is not modified.
    //double 			determinant3by3Matrix(double* rotMat);                                              ///< Helper algebraic function, calculates determinant of 3by3 matrix stored in the double array pointed by the input pointer
    double 			determinant3by3Matrix(boost::numeric::ublas::matrix<double>& Mat);                  ///< Helper algebraic function, calculates determinant of 3by3 boost matrix
    double 			determinant3by3Matrix(gsl_matrix* Mat);                                             ///< Helper algebraic function, calculates determinant of 3by3 gsl matrix
    double 			determinant2by2Matrix(boost::numeric::ublas::matrix<double>& Mat);                  ///< Helper algebraic function, calculates determinant of 2by2 boost matrix
    void			calculateRotationAngleSinCos(std::array<double,3>& u, std::array<double,3>& v, double& c, double& s);       /// Helper algebraic function calculates the sine and cosine of the rotation angle needed to align vector u onto v.
    void    		calculateRotationAxis(const std::array<double,3>& u, const std::array<double,3> &v, std::array<double,3>& rotAx, double c); /// Helper algebraic function calculates the rotation axis needed to align vector u onto v.
    void			constructRotationMatrix(double c, double s, double* rotAx, double* rotMat);         ///< Helper algebraic functio calculates the rotation matrix from input sine, cosine of the rotation angle and the rotation axis. Writes the matrix into the input rotMat
    void			constructRotationMatrix(double c, double s, std::array<double,3>& rotAx, std::array<double,9>& rotMat); ///< Helper algebraic functio calculates the rotation matrix from input sine, cosine of the rotation angle and the rotation axis. Writes the matrix into the input rotMat
    void			rotateVectorByRotationMatrix(double* u,double* rotMat);                             ///< Helper algebraic function rotates the input vector v by rotation matrix rotMat
    void			rotateVectorByRotationMatrix(double* u,gsl_matrix* rotMat);                         ///< Helper algebraic function rotates the input vector v by rotation matrix rotMat
    void			rotateVectorByRotationMatrix(std::array<double,3>& u, std::array<double,9> rotMat); ///< Helper algebraic function rotates the input vector v by rotation matrix rotMat
    void    		CalculateGrowthRotationByF();                                                       ///< This function calculates the rigid body rotation of the element around the z axis of the tissue from hte fecormation gradient.
    void 			calculateTriPointFForRatation();                                                    ///< This function calculates the current deformaiton gradient as averaged at all Gauss points, for rigid body rotation extraction
    void 			setPlasticDeformationIncrement(double xx, double yy, double zz);                    ///< This function sets diagonal of the plastic deformation gradient increment from input values
    void 			growShapeByFg();                                                                    ///< This function updates the current growth deformaiton gradient with the growt/shape change/plastic deformation increments and their respective rotations.
    void 			changeShapeByFsc(double dt);                                                        ///< This function calculates the shape change increment from shape change rates
    void			checkIfInsideEllipseBands(int nMarkerEllipseRanges, std::vector<double> markerEllipseBandXCentres, std::vector<double> markerEllipseBandR1Ranges, std::vector<double> markerEllipseBandR2Ranges, const std::vector<std::unique_ptr<Node>>& Nodes); ///< This function checks if the element is inside any marker bands for perturbatins.
    bool			checkZCappingInRemodelling(bool volumeConserved, double zRemodellingLowerThreshold, double zRemodellingUpperThreshold, gsl_matrix* increment, gsl_matrix* eigenVec);    ///< This function checks if the remodelling of the element in z axis have reached the specified cap.
    void			assignSoftHinge(double lowHingeLimit, double highHingeLimit,double softnessLevel);  ///< This function modulates the stiffness of the hinge domain of the tissue with the input level. The domain is defined in relative x position boundaries.

    void			calculatePlasticDeformation3D(bool volumeConserved, double dt, double plasticDeformationHalfLife, double zRemodellingLowerThreshold, double zRemodellingUpperThreshold); ///< This function calculates the plastic deformation (remodelling) from the current elastic deformation gradient.
    //void 			addMigrationIncrementToGrowthIncrement(gsl_matrix* migrationIncrement);
    void 			displayDebuggingMatrices();                                                                     ///< This function displays a selected set of matricex for debugging purposes.
    virtual double 		getApicalSideLengthAverage(){return ParentErrorMessage("getApicalSideLengthAverage",0.0);}  ///< The virtual function of the parent to calculate average apical side length, dependent on nodal topology, defined in each child.
    virtual double 		getBasalSideLengthAverage(){return ParentErrorMessage("getBasalSideLengthAverage",0.0);}    ///< The virtual function of the parent to calculate average basal side length, dependent on nodal topology, defined in each child.
    virtual int 		getCorrecpondingApical(int /*currNodeId*/){return ParentErrorMessage("getCorrecpondingApical", -100);}  ///< The virtual function of the parent to obtain the corresponding apical node of a basal node, dependent on nodal topology, defined in each child.
    virtual bool 		IsThisNodeMyBasal(int /*currNodeId*/){return ParentErrorMessage("IsThisNodeMyBasal", false);}   ///< The virtual function of the parent to check if the input node ID is a basal node of the element, dependent on nodal topology, defined in each child.
    virtual bool 		IsThisNodeMyApical(int /*currNodeId*/){return ParentErrorMessage("IsThisNodeMyApical", false);} ///< The virtual function of the parent to check if the input node ID is an apical node of the element, dependent on nodal topology, defined in each child.
    virtual double 		getElementHeight(){return ParentErrorMessage("getElementHeight", 0.0);}                         ///< //< The virtual function of the parent to calculate z height of the element, dependent on nodal topology, defined in each child.
    virtual void		constructElementStackList(const int /*discretisationLayers*/, const std::vector<std::unique_ptr<ShapeBase>>& /*elementsList*/){ParentErrorMessage("constructElementStackList");}
    virtual void 		checkRotationConsistency3D(){ParentErrorMessage("checkRotationConsistency3D");}                                         ///< The virtual function of the parent to check if the two input nodes of the element are directly connected on one of the elemental surfaces, dependent on nodal topology, defined in each child.
    virtual bool 		areNodesDirectlyConnected(int /*node0*/, int /*node1*/){return ParentErrorMessage("areNodesDirectlyConnected",false);} ///< The virtual function of the parent to check if the rotation of the nodes of the element are consistent, dependent on nodal topology, defined in each child.
    bool 			DoesPointBelogToMe(int IdNode);                             ///< This function checks if the input node belogs to the element.
    void 			assignVolumesToNodes(const std::vector<std::unique_ptr<Node>>& Nodes);  ///< This function distributes element's total volume among its owner nodes.
    void 			calculateZProjectedAreas();                                         ///< This function calculated the z-projected (to world xy plane) apical and basal areas of the element.
    void 			assignZProjectedAreas(const std::vector<std::unique_ptr<Node>>& Nodes); ///< This function distributes element's z-projected areas among its owner nodes.
    void 			assignElementToConnectedNodes(const std::vector<std::unique_ptr<Node>>& Nodes); ///< This function assigns the element to the nodes it owns, necessary to construc the owner and connectivity list of nodes.
    void 			setECMMimicing(bool IsECMMimicing);                         ///< This funciton sets the element as an ECM mimicking element (distinct domain in terms of physical characteristics).
    void 			setActinMimicing(bool isActinMimicing);                     ///< This funciton sets the element as an actin mimicking element (distinct domain in terms of physical characteristics).
    virtual void 		assignExposedSurfaceAreaIndices(const std::vector<std::unique_ptr<Node>>& /*Nodes*/){ParentErrorMessage("assignExposedSurfaceAreaIndices");} ///< The virtual function on parent assigns the nodes of the surfaces that are exposed to external world, dependent on topology, defined for each child.
    void 			calculateViscositySurfaces();                               ///< This function calls for the assignment of exposed surfaces if the element has viscosity
    void 			assignViscositySurfaceAreaToNodes(const std::vector<std::unique_ptr<Node>>& Nodes); ///< This function distributes elemenbt's exposed surfaces to nodes.

    void 			calculateEmergentRotationAngles();                          ///< This function calculates the emergent rotation of the element in xy plane for display purposes
    void 			updateReferencePositionMatrixFromMeshInput(std::ifstream& file);    ///< This function updates the reference position of the element from save file
    void			fillNodeNeighbourhood(const std::vector<std::unique_ptr<Node>>& Nodes); ///< This function fills in the node neightbourhood, needed for constrction of the connectivity of nodes
    double 			dotProduct3D(std::array<double,3>& u, std::array<double,3>& v); ///< Helper algebraic function, calculates dot product of two arrays <double,3>
    void			crossProduct3D(gsl_vector* u, gsl_vector* v, gsl_vector* cross); ///< Helper algebraic function, calculates cross product of two gsl_vectors, writes into the third input gls vector.
    std::array<double,3> 	crossProduct3D(std::array<double,3> u, std::array<double,3> v); ///< Helper algebraic function, calculates cross product of two arrays <double,3>
    virtual void 		setBasalNeigElementId(const std::vector<std::unique_ptr<ShapeBase>>& /*elementsList*/){ParentErrorMessage("setBasalNeigElementId");}
    bool 			isElementFlippedInPotentialNewShape(int nodeId, double newX, double newY, double newZ); ///< This function checks if the element will plip in the case that its node (nodeID) is moved to the new x,y,z coordintes specified in the input. Necessary in node collapsing.
    void 			checkForCollapsedNodes(int TissueHeightDiscretisationLayers, const std::vector<std::unique_ptr<Node>>& Nodes, const std::vector<std::unique_ptr<ShapeBase>>& Elements); ///< This function checks if any of the edges of the element is shortened to the extent that it should be collapsed.
    bool 			hasEnoughNodesOnCurve(const std::vector<std::unique_ptr<Node> > &Nodes);    ///< This function checks if the majority of the nodes of teh element reside in a curved region, to assign it to specific curvature dependent perturbations.
    void 			assignEllipseBandIdToWholeTissueColumn(size_t TissueHeightDiscretisationLayers, const std::vector<std::unique_ptr<Node>>& Nodes, const std::vector<std::unique_ptr<ShapeBase>>& Elements); ///< This function assigns the marker ID of the apical elemetn ot all its connected elements in the tissue hight (all column of the element).
    void 			assignEllipseBandId(const std::vector<std::unique_ptr<Node>>& Nodes, int selectedEllipseBandId); ///< This function assigns the marking ellipse band ID of the element depending on the definition of nodes it is consturcted of.
    void 			assignEllipseBandIdToNodes(const std::vector<std::unique_ptr<Node>>& Nodes);    ///< This function assigns the marker ID of the elemetn to all its nodes.
    void 			addToElementalElasticSystemForces(int i,int j,double value); 	/// This function is to add the input value, to the (i,j)th element of the ElementalElasticSystemForces
    void 			addToTriPointKe(int i,int j,double value); 			/// This function is to add the input value, to the (i,j)th element of the triPointKe
};

#endif


/**
 * @file batchf_zdotu_sub.c
 *
 * Part of API test for Batched BLAS routines.
 *
 * @author  Samuel  D. Relton
 * @author  Pedro   V. Lara
 * @author  Mawussi Zounon
 * @date
 *
 * @precisions normal z -> c
 *
 **/

#include <cblas.h>
#include "bblas.h"

#define COMPLEX

void batchf_zdotu_sub(
		  const int n,
		  BBLAS_Complex64_t const * const * x,
		  const int incx,
		  BBLAS_Complex64_t const * const * y,
		  const int incy,
		  BBLAS_Complex64_t *dotu,
		  const int batch_count, int *info)
{
	/* Local variables */
	int first_index = 0;
	int batch_iter = 0;
	char func_name[15] = "batchf_zdotu";

	/*initialize the result */
	for (batch_iter = 0; batch_iter < batch_count; batch_iter++)
	  {
	    dotu[batch_iter] = (BBLAS_Complex64_t)0.0;
	  }

	/* Check input arguments */
	if (batch_count < 0)
	{
		xerbla_batch(func_name, BBLAS_ERR_BATCH_COUNT, -1);
	}

	if (n < 0)
	{
		xerbla_batch(func_name, BBLAS_ERR_N, first_index);
		info[first_index] = BBLAS_ERR_N;

	}

	if (incx < 1)
	{
		xerbla_batch(func_name, BBLAS_ERR_INCX, first_index);
		info[first_index] = BBLAS_ERR_INCX;
	}
	if (incy < 1)
	{
		xerbla_batch(func_name, BBLAS_ERR_INCY, first_index);
		info[first_index] = BBLAS_ERR_INCY;
	}
	/* Call CBLAS */
	for (batch_iter = 0; batch_iter < batch_count; batch_iter++)
	{
	  cblas_zdotu_sub(
		      n,
		      (void *)x[batch_iter],
		      incx,
		      (void *)y[batch_iter],
		      incy,
		      &dotu[batch_iter]);
	  /* Successful */
	} /* End fixed size for loop */
	info[first_index] = BBLAS_SUCCESS;
}
#undef COMPLEX


#ifndef __UTILS_UTILS_H_INCLUDED__
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <gsl/gsl>
#include <numeric>
#include <random>
#define __UTILS_UTILS_H_INCLUDED__

template<typename Matrix>
void removeRow(Matrix& matrix, gsl::index rowToRemove)
{
    gsl::index numRows = matrix.rows()-1;
    gsl::index numCols = matrix.cols();

    if( rowToRemove < numRows )
        matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.block(rowToRemove+1,0,numRows-rowToRemove,numCols).eval();

    matrix.conservativeResize(numRows,numCols);
}
template<typename Matrix>
void removeColumn(Matrix& matrix, gsl::index colToRemove)
{
    gsl::index numRows = matrix.rows();
    gsl::index numCols = matrix.cols()-1;

    if( colToRemove < numCols )
        matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove).eval();

    matrix.conservativeResize(numRows,numCols);
}

template <typename T>
void select(std::vector<T>& result, const std::vector<T>& in, const std::vector<typename std::vector<T>::size_type>& s) {
    result.reserve(s.size());
    std::transform(s.begin(), s.end(), std::back_inserter(result),
                   [&in](typename std::vector<T>::size_type idx) {
                       return in.at(idx);
                   });
}

std::tuple<std::vector<gsl::index>,std::vector<gsl::index> > TrainTestSplit(gsl::index nrows,double testRatio=0.3){
    assert(testRatio>=0 && testRatio<=1);
    gsl::index ntest = (gsl::index) nrows*(testRatio);
    gsl::index ntrain = nrows - ntest;
    std::vector<int> idx(nrows);
    std::iota(idx.begin(), idx.end(), 0);

    std::vector<gsl::index> trainIdx, testIdx;
    trainIdx.resize(ntrain);
    testIdx.resize(ntest);
    std::sample(idx.begin(), idx.end(), std::back_inserter(testIdx),
                ntest, std::mt19937{std::random_device{}()});
    std::sort(testIdx.begin(),testIdx.end());
    std::set_difference(idx.begin(), idx.end(), trainIdx.begin(), trainIdx.end(),
                        std::inserter(testIdx, testIdx.begin()));
    return {trainIdx,testIdx};
}

std::tuple<std::vector<gsl::index>,std::vector<gsl::index> > TrainTestSplit(std::vector<gsl::index> inputIdx,double testRatio=0.3){
    assert(testRatio>=0 && testRatio<=1);
    gsl::index nrows = inputIdx.size();
    gsl::index ntest = (gsl::index) nrows*(testRatio);
    gsl::index ntrain = nrows - ntest;
    std::vector<int> idx(nrows);
    std::iota(idx.begin(), idx.end(), 0);

    std::vector<gsl::index> trainIdx, testIdx;
    trainIdx.resize(ntrain);
    testIdx.resize(ntest);
    std::sample(inputIdx.begin(), inputIdx.end(), std::back_inserter(testIdx),
                ntest, std::mt19937{std::random_device{}()});
    std::sort(testIdx.begin(),testIdx.end());
    std::sort(inputIdx.begin(),inputIdx.end());
    std::set_difference(inputIdx.begin(), inputIdx.end(), trainIdx.begin(), trainIdx.end(),
                        std::inserter(testIdx, testIdx.begin()));
    return {trainIdx,testIdx};
}

template<typename T>
std::vector<T> linspace(T start_in, T end_in, int num_in)
{

    std::vector<T> linspaced;

    double start = static_cast<double>(start_in);
    double end = static_cast<double>(end_in);
    double num = static_cast<double>(num_in);

    if (num == 0) { return linspaced; }
    if (num == 1)
    {
        linspaced.push_back(start);
        return linspaced;
    }

    double delta = (end - start) / (num - 1);
    for(int i=0; i < num-1; ++i)
    {
        linspaced.push_back(start + delta * i);
    }
    linspaced.push_back(end);
    return linspaced;
}

template<typename T>
void print_vector(std::vector<T> vec)
{
    std::cout << "size: " << vec.size() << std::endl;
    for (T & d : vec)
        std::cout << d << " ";
    std::cout << std::endl;
}
#endif


/* multiroots/gsl_multiroots.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 2 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_MULTIROOTS_H__
#define __GSL_MULTIROOTS_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.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

/* Definition of vector-valued functions with parameters based on gsl_vector */

struct gsl_multiroot_function_struct
{
  int (* f) (const gsl_vector * x, void * params, gsl_vector * f);
  size_t n;
  void * params;
};

typedef struct gsl_multiroot_function_struct gsl_multiroot_function ;

#define GSL_MULTIROOT_FN_EVAL(F,x,y) (*((F)->f))(x,(F)->params,(y))

int gsl_multiroot_fdjacobian (gsl_multiroot_function * F,
                              const gsl_vector * x, const gsl_vector * f,
                              double epsrel, gsl_matrix * jacobian);

typedef struct
  {
    const char *name;
    size_t size;
    int (*alloc) (void *state, size_t n);
    int (*set) (void *state, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
    int (*iterate) (void *state, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
    void (*free) (void *state);
  }
gsl_multiroot_fsolver_type;

typedef struct
  {
    const gsl_multiroot_fsolver_type * type;
    gsl_multiroot_function * function ;
    gsl_vector * x ;
    gsl_vector * f ;
    gsl_vector * dx ;
    void *state;
  }
gsl_multiroot_fsolver;

gsl_multiroot_fsolver *
gsl_multiroot_fsolver_alloc (const gsl_multiroot_fsolver_type * T,
                                     size_t n);

void gsl_multiroot_fsolver_free (gsl_multiroot_fsolver * s);

int gsl_multiroot_fsolver_set (gsl_multiroot_fsolver * s,
                               gsl_multiroot_function * f, gsl_vector * x);

int gsl_multiroot_fsolver_iterate (gsl_multiroot_fsolver * s);

const char * gsl_multiroot_fsolver_name (const gsl_multiroot_fsolver * s);
gsl_vector * gsl_multiroot_fsolver_root (const gsl_multiroot_fsolver * s);
gsl_vector * gsl_multiroot_fsolver_dx (const gsl_multiroot_fsolver * s);
gsl_vector * gsl_multiroot_fsolver_f (const gsl_multiroot_fsolver * s);

/* Definition of vector-valued functions and gradient with parameters
   based on gsl_vector */

struct gsl_multiroot_function_fdf_struct
{
  int (* f) (const gsl_vector * x, void * params, gsl_vector * f);
  int (* df) (const gsl_vector * x, void * params, gsl_matrix * df);
  int (* fdf) (const gsl_vector * x, void * params, gsl_vector * f, gsl_matrix *df);
  size_t n;
  void * params;
};

typedef struct gsl_multiroot_function_fdf_struct gsl_multiroot_function_fdf ;

#define GSL_MULTIROOT_FN_EVAL_F(F,x,y) ((*((F)->f))(x,(F)->params,(y)))
#define GSL_MULTIROOT_FN_EVAL_DF(F,x,dy) ((*((F)->df))(x,(F)->params,(dy)))
#define GSL_MULTIROOT_FN_EVAL_F_DF(F,x,y,dy) ((*((F)->fdf))(x,(F)->params,(y),(dy)))

typedef struct
  {
    const char *name;
    size_t size;
    int (*alloc) (void *state, size_t n);
    int (*set) (void *state, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
    int (*iterate) (void *state, gsl_multiroot_function_fdf * fdf, gsl_vector * x, gsl_vector * f, gsl_matrix * J, gsl_vector * dx);
    void (*free) (void *state);
  }
gsl_multiroot_fdfsolver_type;

typedef struct
  {
    const gsl_multiroot_fdfsolver_type * type;
    gsl_multiroot_function_fdf * fdf ;
    gsl_vector * x;
    gsl_vector * f;
    gsl_matrix * J;
    gsl_vector * dx;
    void *state;
  }
gsl_multiroot_fdfsolver;

gsl_multiroot_fdfsolver *
gsl_multiroot_fdfsolver_alloc (const gsl_multiroot_fdfsolver_type * T,
                                      size_t n);

int
gsl_multiroot_fdfsolver_set (gsl_multiroot_fdfsolver * s,
                             gsl_multiroot_function_fdf * fdf,
                             gsl_vector * x);

int
gsl_multiroot_fdfsolver_iterate (gsl_multiroot_fdfsolver * s);

void
gsl_multiroot_fdfsolver_free (gsl_multiroot_fdfsolver * s);

const char * gsl_multiroot_fdfsolver_name (const gsl_multiroot_fdfsolver * s);
gsl_vector * gsl_multiroot_fdfsolver_root (const gsl_multiroot_fdfsolver * s);
gsl_vector * gsl_multiroot_fdfsolver_dx (const gsl_multiroot_fdfsolver * s);
gsl_vector * gsl_multiroot_fdfsolver_f (const gsl_multiroot_fdfsolver * s);

int gsl_multiroot_test_delta (const gsl_vector * dx, const gsl_vector * x,
                              double epsabs, double epsrel);

int gsl_multiroot_test_residual (const gsl_vector * f, double epsabs);

GSL_VAR const gsl_multiroot_fsolver_type * gsl_multiroot_fsolver_dnewton;
GSL_VAR const gsl_multiroot_fsolver_type * gsl_multiroot_fsolver_broyden;
GSL_VAR const gsl_multiroot_fsolver_type * gsl_multiroot_fsolver_hybrid;
GSL_VAR const gsl_multiroot_fsolver_type * gsl_multiroot_fsolver_hybrids;

GSL_VAR const gsl_multiroot_fdfsolver_type * gsl_multiroot_fdfsolver_newton;
GSL_VAR const gsl_multiroot_fdfsolver_type * gsl_multiroot_fdfsolver_gnewton;
GSL_VAR const gsl_multiroot_fdfsolver_type * gsl_multiroot_fdfsolver_hybridj;
GSL_VAR const gsl_multiroot_fdfsolver_type * gsl_multiroot_fdfsolver_hybridsj;

__END_DECLS

#endif /* __GSL_MULTIROOTS_H__ */


/*
 * CARP/Kaczmarz sweep on diagonally banded matrix. The essential loop is:
 *
 * for each row i
 *     x = x + w*(b(i) - A(i,:)*x)*A(i,:)'
 * end
 *
 * The matrix is given in band storage format, where each row (stored
 * contiguously in memory) of the array R stores a diagonal of the matrix with
 * offset idx(j), such that
 *
 * 	   A(i,i+idx(j)) = R(i,j)
 *
 * use (from MATLAB):
 * 		y = sweepR_mex(R,idx,x,b,w,dir)
 *
 * 		R			- matrix of diagonals of matrix A
 *		idx			- offsets of diagonals
 *		x			- initial guess
 * 		b 			- right hand side (source)
 *		w 			- relaxation parameter (0 <= w <= 2)
 *		dir 		- if dir > 0, go through matrix rows in ascending order.
 * 			  		  if dir < 0, go through matrix rows in descending order.
 * 		n_threads	- OPTIONAL argument to control the number of execution
 * 		threads solving CARP blocks in parallel. The number of threads can also
 * 		be defined via an environment variable (OMP_NUM_THREADS), but this
 * 		optional argument takes precedence. The default number of threads is
 * 		one. Take care if using more than one MATLAB worker per node: each
 * 		MATLAB worker will use OMP_NUM_THREADS, so if there are four workers on
 * 		a node, there will be 4 x OMP_NUM_THREADS parallel CARP sweeps.
 *
 * Author: Art Petrenko, Tristan van Leeuwen
 *         Seismic Laboratory for Imaging and Modeling
 *         Department of Earth, Ocean, and Atmosperic Sciences
 *         The University of British Columbia
 *
 * Date: July, 2014

 * You may use this code only under the conditions and terms of the
 * license contained in the file LICENSE provided with this source
 * code. If you do not agree to these terms you may not use this
 * software.
*/

#include <stdlib.h> /* for getenv */
#include <stddef.h> /* for size_t type */
#include <string.h> /* for memcpy */
#include <pthread.h> /* for threading */

/* The following section allows this file to compile on Mac OS X 10.8.5. Pass
 * the flag -DARCH_MACI64 to the compiler to activate it. */
#ifdef ARCH_MACI64
#include "pthread_barrier.h"
#include <mach/error.h>
typedef wchar_t char16_t;
#else /* not ARCH_MACI64 */
#include <error.h>
#endif /* ARCH_MACI64 */

#include <math.h>
#include <mex.h>
#include <matrix.h>
#include <blas.h>

struct copy_init_guess_data_t
{
	double *copy_src_real, *copy_dst_real;
	double *copy_src_imag, *copy_dst_imag;
	long n_to_copy;
};

struct sweep_data_t
{
	long start_row, end_row, ncol, ny, nx, haloWidth, main_diagonal_offset;
	double *Rr, *Ri, *yr, *yi, *br, *bi;
	long *idx;
	double w;
	int dir;
};

struct average_data_t
{
	double *copy_src_real, *copy_dst_real;
	double *copy_src_imag, *copy_dst_imag;
	double *halo_1_real, *halo_2_real;
	double *halo_1_imag, *halo_2_imag;
	double *halo_dst_real, *halo_dst_imag;
	long n_to_copy, n_in_halo;
};

struct thread_data_t
{
	struct copy_init_guess_data_t copy_init_guess_data;
	struct sweep_data_t sweep_data;
	struct average_data_t average_data;
	pthread_barrier_t *barrier;
};

void *do_sweep(void *thread_args_void)
{
    struct sweep_data_t *thread_args;
	/* Variables contained in thread_args_void struct */
	long start_row, end_row, haloWidth, ncol, ny, nx;
	/* Rr and Ri are pointers to short fat ncol-by-N matrices */
	double *Rr, *Ri, *yr, *yi, *br, *bi;
	long *idx;
	double w;
	int dir;
	/* Temporary storage variables */
	double cr = 0, ci = 0;
	long offset, main_diagonal_offset;

	/* Assign local pointers to data locations in shared memory */
	thread_args = (struct sweep_data_t *) thread_args_void;
	start_row 	= thread_args->start_row;
	end_row 	= thread_args->end_row;
	ncol 		= thread_args->ncol;
	ny 			= thread_args->ny;
	nx			= thread_args->nx;
	haloWidth	= thread_args->haloWidth;
	main_diagonal_offset = thread_args->main_diagonal_offset;
	Rr 			= thread_args->Rr;
	Ri 			= thread_args->Ri;
	idx 		= thread_args->idx;
	yr 			= thread_args->yr;
	yi 			= thread_args->yi;
	br 			= thread_args->br;
	bi 			= thread_args->bi;
	w 			= thread_args->w;
	dir 		= thread_args->dir;

	offset = (start_row == 0 ? 0 : haloWidth - main_diagonal_offset);

	/* Kaczmarz sweep on one row block */
	for(long i = (dir > 0 ? start_row : end_row-1);
		dir > 0 ? i<end_row : i>=start_row;
		dir > 0 ? i++ : i--)
	{

		if (0 <= i + main_diagonal_offset && i + main_diagonal_offset < nx){
			cr = br[i + main_diagonal_offset];
			ci = bi[i + main_diagonal_offset];
		}
		else{
		  //error(1,0,"Discovery of whether the iterate vector is haloed failed.");
		}
		/* First loop over non-zero row elements calculates inner product
		 * of matrix row and CARP iterate */
		for(long j=0, k; j<ncol;j++){
			/* i + idx[j] is the column index for the full Helmholtz matrix.
			 * k is the index into the vector representing the CARP iterate
			 * of the given block. */
			k = i + idx[j] - start_row + offset;
			if(0<=k && k<ny){
				cr -= Rr[i*ncol + j]*yr[k] - Ri[i*ncol + j]*yi[k];
				ci -= Rr[i*ncol + j]*yi[k] + Ri[i*ncol + j]*yr[k];
			}
		}
		/* Second loop over non-zero row elements updates CARP iterate */
		cr *= w;
		ci *= w;
		for(long j=0, k; j<ncol;j++){
			k = i + idx[j] - start_row + offset;
			if(0<=k && k<ny){
				yr[k] +=   cr*Rr[i*ncol + j] + ci*Ri[i*ncol + j];
				yi[k] +=  -cr*Ri[i*ncol + j] + ci*Rr[i*ncol + j];
			}
		}
	}

	return NULL;
}

void *average_halos(void *thread_args_void)
{
	struct average_data_t *thread_args;
	double *copy_src_real, *copy_dst_real;
	double *copy_src_imag, *copy_dst_imag;
	double *halo_1_real, *halo_2_real;
	double *halo_1_imag, *halo_2_imag;
	double *halo_dst_real, *halo_dst_imag;
	size_t n_to_copy, n_in_halo;

	/* Assign local pointer to data locations in shared memory */
	thread_args 	= (struct average_data_t *) thread_args_void;
	copy_src_real	= thread_args->copy_src_real;
	copy_dst_real	= thread_args->copy_dst_real;
	copy_src_imag	= thread_args->copy_src_imag;
	copy_dst_imag	= thread_args->copy_dst_imag;
	n_to_copy		= thread_args->n_to_copy;
	halo_1_real     = thread_args->halo_1_real;
	halo_2_real		= thread_args->halo_2_real;
	halo_dst_real	= thread_args->halo_dst_real;
	halo_1_imag		= thread_args->halo_1_imag;
	halo_2_imag		= thread_args->halo_2_imag;
	halo_dst_imag   = thread_args->halo_dst_imag;
	n_in_halo		= thread_args->n_in_halo;

	/* Copy the non-halo parts of the domain block directly to output array */
	memcpy((void *)copy_dst_real, (void *)copy_src_real, sizeof(double)*n_to_copy);
	memcpy((void *)copy_dst_imag, (void *)copy_src_imag, sizeof(double)*n_to_copy);

	/* Average the halo parts of the domain block and copy to output array.
	 * NOTE: this assumes domain blocks overlap only with their nearest
	 * neighbour segments. */
	for (long i = 0; i < n_in_halo; i++){
		halo_dst_real[i] = (halo_1_real[i] + halo_2_real[i]) / 2;
		halo_dst_imag[i] = (halo_1_imag[i] + halo_2_imag[i]) / 2;
	}

	return NULL;
}

void *sweep_and_average(void *thread_data_void)
{
	struct thread_data_t *thread_data = (struct thread_data_t *) thread_data_void;

	/* Copy the initial guess into the working arrays */
	memcpy((void *)thread_data->copy_init_guess_data.copy_dst_real, (void *)thread_data->copy_init_guess_data.copy_src_real, sizeof(double)*thread_data->copy_init_guess_data.n_to_copy);
	memcpy((void *)thread_data->copy_init_guess_data.copy_dst_imag, (void *)thread_data->copy_init_guess_data.copy_src_imag, sizeof(double)*thread_data->copy_init_guess_data.n_to_copy);
	do_sweep((void *) &(thread_data->sweep_data));

	pthread_barrier_wait(thread_data->barrier);

	average_halos((void *) &(thread_data->average_data));

	return NULL;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	/* structs to hold all arguments to each thread in one variable */
	struct sweep_data_t **thread_args_sweep = NULL;
	struct average_data_t **thread_args_average = NULL;
	struct thread_data_t **thread_data = NULL;
	struct copy_init_guess_data_t **copy_init_guess_data = NULL;
	pthread_barrier_t barrier_after_sweep_before_halo_average;

	mwSize ncol, nx;
	ptrdiff_t ncolBlas, idxIncBlas = 1, maxIdxLoc = 0;
	mwSize haloWidth;
	mwSize n_threads = 1;
	char *n_threads_str = NULL;
	double *Rr,*Ri,*idxd = NULL,*xr,*xi,*br,*bi,*yr,*yi;
	long *idx = NULL;
    double w = 0;
	int dir = 1;
	mwSize N=1, numGridPointsPerBlock, main_diagonal_offset;

	/* Flags that are set if memory is allocated within the MEX file */
	int Ri_alloc=0, xi_alloc=0, bi_alloc=0;

	/* a return code flag and segment demarcation arrays */
	mwSize i_thread;
	mwSize *seg_bounds_hi, *seg_bounds_mid, *seg_bounds_row, *seg_bounds_lo;

	/* Threading variables */
	pthread_t *threadIDs = NULL;
	pthread_attr_t attr;

	/* Arrays to hold (overlapping) segments of yr and yi, a pair for each
	 * thread */
	double **yr_seg = NULL, **yi_seg = NULL;

	/* Allow worker threads to join back to main thread once they are done */
	pthread_attr_init(&attr);
	pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);

	/* Read input arguments; initialize complex part to zero if input is real. */
	N = mxGetN(prhs[0]);
    ncol = mxGetM(prhs[0]);
	ncolBlas = (ptrdiff_t)ncol;
	Rr  = mxGetPr(prhs[0]);
    if(mxIsComplex(prhs[0])){
        Ri  = mxGetPi(prhs[0]);
    }
    else{
        Ri = mxCalloc(N*ncol,sizeof(double));
		Ri_alloc = 1;
    }
	idxd = mxGetPr(prhs[1]);
    nx  = mxGetM(prhs[2]);
	xr  = mxGetPr(prhs[2]);
    if(mxIsComplex(prhs[2])){
        xi  = mxGetPi(prhs[2]);
    }
    else{
        xi = mxCalloc(nx,sizeof(double));
		xi_alloc = 1;
    }
	br  = mxGetPr(prhs[3]);
    if(mxIsComplex(prhs[3])){
        bi  = mxGetPi(prhs[3]);
    }
    else{
        bi = mxCalloc(nx,sizeof(double));
		bi_alloc = 1;
    }
	if (mxGetM(prhs[3]) != nx){
	    mexPrintf('%d %d \n',mxGetM(prhs[3]),nx);
		mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:NumElements",
				"The number of elements in the iterate and right hand side vectors must be equal.");
	}
	w   = mxGetScalar(prhs[4]);
	dir = lrint(mxGetScalar(prhs[5]));

	/* The default value for the number of threads can be overridden by an
	 * environment variable. */
	n_threads_str = getenv("OMP_NUM_THREADS");
	if (n_threads_str == NULL){
		n_threads = 1;
	}
	else{
		n_threads = strtol(n_threads_str, NULL, 10);
		if(n_threads < 1){
			n_threads = 1;
		}
	}
	/* The environment variable can in turn be overridden by an optional
	 * argument to the mexFunction. */
	if (nrhs >= 7){
		if(1 <= lrint(mxGetScalar(prhs[6]))){
			n_threads = lrint(mxGetScalar(prhs[6]));
		}
	}

	/* Allocate the final output vector */
	plhs[0]  = mxCreateDoubleMatrix(nx, 1, mxCOMPLEX);
	yr       = mxGetPr(plhs[0]);
    yi       = mxGetPi(plhs[0]);

	/* Check to make sure memory was allocated correctly */
	if (Rr==NULL || Ri==NULL || idxd==NULL || xr==NULL || xi==NULL || br==NULL || bi==NULL ||
	    yr==NULL || yi==NULL){
		mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:OutOfMemory",
				"Could not allocate memory for main computational variables.");
	}

	if ((idx = (long *) mxCalloc(ncol, sizeof(long))) == NULL){
		mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:OutOfMemory",
				"Could not allocate memory for main computational variables.");
	}
	for (mwSize i=0; i < ncol; i++){
		idx[i] = lrint(idxd[i]);
	}

	/* Compute (half) halo width. Remember that BLAS routines like idamax
	 * return FORTRAN-style indices. */
	maxIdxLoc = idamax_(&ncolBlas, idxd, &idxIncBlas) - 1;
	haloWidth = (mwSize)labs(idx[maxIdxLoc]);

	/* Partition the iterate vector into blocks. Note that the below
	 * partitioning scheme is slighlty different from that in pCARPCG.m in this
	 * directory. The partitioning scheme of pCARPCG corresponds to
	 * distributing a three dimensional array with dimensions given by n
	 * according to Matlab's codistributor1d.defaultPartition(n(3)), and then
	 * vectorizing it. The partition scheme of the present file instead uses
	 * Matlab's codistributor1d.defaultPartion(prod(n)). In other words,
	 * pCARPCG divides the iterate into blocks along the slow dimension,
	 * whereas sweepR_mex.c does not take dimensionality into account, only the
	 * total number of gridpoints. This is done to avoid needing an extra input
	 * parameter with the the system dimensions. The seg_bounds_hi, _lo and
	 * _mid arrays contain indices into non-haloed vectors, while the
	 * seg_bounds_row array contains indices to the rows of the system matrix.
	 *
 	 * yr_seg[i_thread-1]        overlap      yr_seg[i_thread]
	 * ------------------------|-----|-----|-------------------------------
	 *         .----------------^     |     ^-------------------.
	 *    seg_bounds_lo[i_thread], seg_bounds_mid[i_thread], seg_bounds_hi[i_thread]
	 */
	numGridPointsPerBlock = N / n_threads;
	seg_bounds_hi  = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	seg_bounds_mid = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	seg_bounds_lo  = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	seg_bounds_row = (mwSize *)mxCalloc(n_threads+1,sizeof(mwSize));
	if (N == nx){
		main_diagonal_offset = 0;
	}
	else{
		/* The vector is haloed. We are only able to correctly process matrices
		 * with a non-zero main diagonal and symmetric off-main diagonal offsets. */
		if (ncol % 2 != 1){
			mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:EvenNumberOfDiags",
					"Input iterate vector appears to be haloed but there is an even number of non-zero diagonals in the system matrix.");
		}
		main_diagonal_offset = idx[ncol/2];
		for (mwSize i = 1; i <= ncol/2; i++){
			if (idx[ncol/2 + i] - main_diagonal_offset != -(idx[ncol/2 - i] - main_diagonal_offset)){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:DiagsNotSymmetric",
						"Input iterate vector appears to be haloed but the pattern of non-zero diagonals in the system matrix is not symmetric.");
			}
		}
	}
	for (i_thread=0; i_thread<n_threads; i_thread++){
		/* First domain block */
		if (i_thread==0){
			seg_bounds_hi[i_thread]  = 0;
			seg_bounds_mid[i_thread] = 0;
			seg_bounds_row[i_thread] = 0;
			seg_bounds_lo[i_thread]  = 0;
		}
		/* Other domain blocks */
		else {
			if (i_thread <= N % n_threads) {
				seg_bounds_mid[i_thread] = (numGridPointsPerBlock+1)*i_thread + main_diagonal_offset;
				seg_bounds_row[i_thread] = (numGridPointsPerBlock+1)*i_thread;
				seg_bounds_hi[i_thread]  = (numGridPointsPerBlock+1)*i_thread + haloWidth + main_diagonal_offset;
				seg_bounds_lo[i_thread]  = (numGridPointsPerBlock+1)*i_thread - haloWidth + main_diagonal_offset;
			}
			else{
				seg_bounds_mid[i_thread] = (N % n_threads) + numGridPointsPerBlock*i_thread + main_diagonal_offset;
				seg_bounds_row[i_thread] = (N % n_threads) + numGridPointsPerBlock*i_thread;
				seg_bounds_hi[i_thread]  = (N % n_threads) + numGridPointsPerBlock*i_thread + haloWidth + main_diagonal_offset;
				seg_bounds_lo[i_thread]  = (N % n_threads) + numGridPointsPerBlock*i_thread - haloWidth + main_diagonal_offset;
			}
			/* Check that halos do not overlap each other */
			if (seg_bounds_lo[i_thread] < seg_bounds_hi[i_thread-1]){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:TooManyThreads",
						"Too many threads; non-adjacent domain blocks share nearest-neighbour grid points.");
			}
		}
	}
	seg_bounds_lo[n_threads]  = nx;
	seg_bounds_hi[n_threads]  = nx;
	seg_bounds_mid[n_threads] = nx;
	seg_bounds_row[n_threads] = N;

	/* Allocate pointers to segments, to thread arguments and thread IDs */
	thread_args_sweep = (struct sweep_data_t **)mxCalloc(n_threads, sizeof(struct sweep_data_t *));
	if (n_threads > 1) {
		/* Set up a barrier for synchronization of all threads save the master that
		 * executes mexFunction. Note that strictly speaking, only threads working
		 * on domain blocks that share halos need to synchronize with each other.
		 * */
		if (pthread_barrier_init(&barrier_after_sweep_before_halo_average, NULL, n_threads)){
			mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreads",
					"Could not initialize pthread barrier.");
		}

		thread_data = (struct thread_data_t **)mxCalloc(n_threads, sizeof(struct thread_data_t *));
		thread_args_average = (struct average_data_t **)mxCalloc(n_threads, sizeof(struct average_data_t *));
		copy_init_guess_data = (struct copy_init_guess_data_t **)mxCalloc(n_threads, sizeof(struct copy_init_guess_data_t *));
		yr_seg = (double **)mxCalloc(n_threads,sizeof(double *));
		yi_seg = (double **)mxCalloc(n_threads,sizeof(double *));
		threadIDs = (pthread_t *)mxCalloc(n_threads,sizeof(pthread_t));
	}
	for (i_thread=0; i_thread<n_threads; i_thread++){
		/* Allocate the segments for each thread */
		if (n_threads > 1) {
			yr_seg[i_thread] = (double *)mxCalloc((seg_bounds_hi[i_thread+1]-seg_bounds_lo[i_thread]),sizeof(double));
			yi_seg[i_thread] = (double *)mxCalloc((seg_bounds_hi[i_thread+1]-seg_bounds_lo[i_thread]),sizeof(double));
			/* Check that segments were allocated correctly */
			if (yr_seg[i_thread]==NULL || yi_seg[i_thread]==NULL){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreadsOutOfMemory",
						"Could not allocate memory for thread computational variables.");
			}
			copy_init_guess_data[i_thread] = (struct copy_init_guess_data_t *)mxCalloc(1,sizeof(struct copy_init_guess_data_t));
			copy_init_guess_data[i_thread]->copy_src_real = &xr[seg_bounds_lo[i_thread]];
			copy_init_guess_data[i_thread]->copy_src_imag = &xi[seg_bounds_lo[i_thread]];
			copy_init_guess_data[i_thread]->copy_dst_real = yr_seg[i_thread];
			copy_init_guess_data[i_thread]->copy_dst_imag = yi_seg[i_thread];
			copy_init_guess_data[i_thread]->n_to_copy = (size_t) (seg_bounds_hi[i_thread+1] - seg_bounds_lo[i_thread]);
		}

		/* Set thread arguments */
		thread_args_sweep[i_thread] = (struct sweep_data_t *)mxCalloc(1,sizeof(struct sweep_data_t));
		thread_args_sweep[i_thread]->start_row 	= seg_bounds_row[i_thread];
		thread_args_sweep[i_thread]->end_row   	= seg_bounds_row[i_thread+1];
		thread_args_sweep[i_thread]->ncol 	 	= ncol;
		thread_args_sweep[i_thread]->ny 	 	= seg_bounds_hi[i_thread+1]-seg_bounds_lo[i_thread];
		thread_args_sweep[i_thread]->nx			= nx;
		thread_args_sweep[i_thread]->haloWidth 	= haloWidth*(i_thread ? 1 : 0);
		thread_args_sweep[i_thread]->main_diagonal_offset = main_diagonal_offset;
		thread_args_sweep[i_thread]->Rr  		= Rr;
		thread_args_sweep[i_thread]->Ri  		= Ri;
		thread_args_sweep[i_thread]->idx 		= idx;
		if (n_threads > 1){
			thread_args_sweep[i_thread]->yr 	= yr_seg[i_thread];
			thread_args_sweep[i_thread]->yi 	= yi_seg[i_thread];
		}
		else{
			thread_args_sweep[i_thread]->yr	 	= yr;
			thread_args_sweep[i_thread]->yi	 	= yi;
		}
		thread_args_sweep[i_thread]->br 		= br;
		thread_args_sweep[i_thread]->bi 		= bi;
		thread_args_sweep[i_thread]->w 		 	= w;
		thread_args_sweep[i_thread]->dir 		= dir;

		if (n_threads > 1){
			/* Set the arguments for the averaging threads. Note that each
			 * thread is responsible for averaging the low end of its address
			 * range. The middle of its address range is copied, while the high
			 * end of its address range is left for the next thread. */
			thread_args_average[i_thread] = (struct average_data_t *)mxCalloc(1,sizeof(struct average_data_t));
			thread_args_average[i_thread]->copy_src_real = &(yr_seg[i_thread][seg_bounds_hi[i_thread] - seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->copy_dst_real = &(yr[seg_bounds_hi[i_thread]]);
			thread_args_average[i_thread]->copy_src_imag = &(yi_seg[i_thread][seg_bounds_hi[i_thread] - seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->copy_dst_imag = &(yi[seg_bounds_hi[i_thread]]);
			thread_args_average[i_thread]->n_to_copy 	 = (size_t) (seg_bounds_lo[i_thread+1] - seg_bounds_hi[i_thread]);
			thread_args_average[i_thread]->halo_1_real   = &(yr_seg[i_thread-1 >= 0 ? i_thread-1 : 0][seg_bounds_lo[i_thread] - seg_bounds_lo[i_thread-1 >= 0 ? i_thread-1 : 0]]);
			thread_args_average[i_thread]->halo_2_real	 = yr_seg[i_thread];
			thread_args_average[i_thread]->halo_dst_real = &(yr[seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->halo_1_imag   = &(yi_seg[i_thread-1 >= 0 ? i_thread-1 : 0][seg_bounds_lo[i_thread] - seg_bounds_lo[i_thread-1 >= 0 ? i_thread-1 : 0]]);
			thread_args_average[i_thread]->halo_2_imag	 = yi_seg[i_thread];
			thread_args_average[i_thread]->halo_dst_imag = &(yi[seg_bounds_lo[i_thread]]);
			thread_args_average[i_thread]->n_in_halo 	 = (size_t) (seg_bounds_hi[i_thread] - seg_bounds_lo[i_thread]);

			thread_data[i_thread] = (struct thread_data_t *)mxCalloc(1,sizeof(struct thread_data_t));
			thread_data[i_thread]->copy_init_guess_data = *copy_init_guess_data[i_thread];
			thread_data[i_thread]->sweep_data = *thread_args_sweep[i_thread];
			thread_data[i_thread]->average_data = *thread_args_average[i_thread];
			thread_data[i_thread]->barrier = &barrier_after_sweep_before_halo_average;
		}
	}

	if (n_threads == 1) {
		/* Set the initial guess directly in the output array too */
		memcpy((void *)yr, (void *)xr, sizeof(double)*nx);
		memcpy((void *)yi, (void *)xi, sizeof(double)*nx);
		do_sweep((void *)thread_args_sweep[0]);
	}
	else{
		/* Sweep and average, in separate worker threads */
		for (i_thread=0; i_thread<n_threads; i_thread++){
			if (pthread_create(&threadIDs[i_thread], &attr, sweep_and_average, (void *)thread_data[i_thread])){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreadCreate",
						"pthread_create returned non-zero.");
			}
		}

		/* Wait for threads to finish */
		for (i_thread=0; i_thread<n_threads; i_thread++){
			if (pthread_join(threadIDs[i_thread], NULL)){
				mexErrMsgIdAndTxt("SLIM_release_apps:tools:algorithms:ThreeDFreqModeling:sweepR_mex:pthreadCreate",
						"pthread_join returned non-zero.");
			}
		}
	}

	/* Free memory if it was allocated within the MEX file. */
	if (Ri_alloc){
		mxFree(Ri);
	}
	if (xi_alloc){
		mxFree(xi);
	}
	if (bi_alloc){
		mxFree(bi);
	}
	mxFree(seg_bounds_hi);
	mxFree(seg_bounds_mid);
	mxFree(seg_bounds_row);
	mxFree(seg_bounds_lo);
	if (n_threads > 1) {
		mxFree(threadIDs);
	}
	for (i_thread=0; i_thread<n_threads; i_thread++){
		mxFree(thread_args_sweep[i_thread]);
		if (n_threads > 1){
			mxFree(thread_args_average[i_thread]);
			mxFree(thread_data[i_thread]);
			mxFree(copy_init_guess_data[i_thread]);
			mxFree(yr_seg[i_thread]);
			mxFree(yi_seg[i_thread]);
		}
	}
	if (n_threads > 1){
		pthread_barrier_destroy(&barrier_after_sweep_before_halo_average);
		mxFree(thread_args_average);
		mxFree(thread_data);
		mxFree(copy_init_guess_data);
		mxFree(yr_seg);
		mxFree(yi_seg);
	}
	mxFree(idx);
	mxFree(thread_args_sweep);
	pthread_attr_destroy(&attr);

	/* Don't think I need pthread_exit() here, because pthread_join is called above */
	return;
}