#include <ceed.h>
#include <petsc.h>
#include "../problems/neo-hookean.h"

// Build libCEED context object
PetscErrorCode PhysicsContext_NH(MPI_Comm comm, Ceed ceed, Units *units,
                                 CeedQFunctionContext *ctx) {
  PetscErrorCode ierr;
  Physics_NH phys;

  PetscFunctionBegin;

  ierr = PetscMalloc1(1, units); CHKERRQ(ierr);
  ierr = PetscMalloc1(1, &phys); CHKERRQ(ierr);
  ierr = ProcessPhysics_NH(comm, phys, *units); CHKERRQ(ierr);
  CeedQFunctionContextCreate(ceed, ctx);
  CeedQFunctionContextSetData(*ctx, CEED_MEM_HOST, CEED_COPY_VALUES,
                              sizeof(*phys), phys);
  ierr = PetscFree(phys); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// Build libCEED smoother context object
PetscErrorCode PhysicsSmootherContext_NH(MPI_Comm comm, Ceed ceed,
    CeedQFunctionContext ctx, CeedQFunctionContext *ctx_smoother) {
  PetscErrorCode ierr;
  PetscScalar nu_smoother = 0;
  PetscBool nu_flag = PETSC_FALSE;
  Physics_NH phys, phys_smoother;

  PetscFunctionBegin;

  ierr = PetscOptionsBegin(comm, NULL,
                           "Neo-Hookean physical parameters for smoother", NULL);
  CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-nu_smoother", "Poisson's ratio for smoother",
                            NULL, nu_smoother, &nu_smoother, &nu_flag);
  CHKERRQ(ierr);

  ierr = PetscOptionsEnd(); CHKERRQ(ierr); // End of setting Physics

  if (nu_flag) {
    // Copy context
    CeedQFunctionContextGetData(ctx, CEED_MEM_HOST, &phys);
    ierr = PetscMalloc1(1, &phys_smoother); CHKERRQ(ierr);
    ierr = PetscMemcpy(phys_smoother, phys, sizeof(*phys)); CHKERRQ(ierr);
    CeedQFunctionContextRestoreData(ctx, &phys);
    // Create smoother context
    CeedQFunctionContextCreate(ceed, ctx_smoother);
    phys_smoother->nu = nu_smoother;
    CeedQFunctionContextSetData(*ctx_smoother, CEED_MEM_HOST, CEED_COPY_VALUES,
                                sizeof(*phys_smoother), phys_smoother);
    ierr = PetscFree(phys_smoother); CHKERRQ(ierr);
  } else {
    *ctx_smoother = NULL;
  }

  PetscFunctionReturn(0);
}

// Process physics options - Neo-Hookean
PetscErrorCode ProcessPhysics_NH(MPI_Comm comm, Physics_NH phys, Units units) {
  PetscErrorCode ierr;
  PetscBool nu_flag = PETSC_FALSE;
  PetscBool Young_flag = PETSC_FALSE;
  phys->nu = 0;
  phys->E = 0;
  units->meter     = 1;        // 1 meter in scaled length units
  units->second    = 1;        // 1 second in scaled time units
  units->kilogram  = 1;        // 1 kilogram in scaled mass units

  PetscFunctionBeginUser;

  ierr = PetscOptionsBegin(comm, NULL, "Neo-Hookean physical parameters", NULL);
  CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-nu", "Poisson's ratio", NULL, phys->nu, &phys->nu,
                            &nu_flag); CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-E", "Young's Modulus", NULL, phys->E, &phys->E,
                            &Young_flag); CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-units_meter", "1 meter in scaled length units",
                            NULL, units->meter, &units->meter, NULL);
  CHKERRQ(ierr);
  units->meter = fabs(units->meter);

  ierr = PetscOptionsScalar("-units_second", "1 second in scaled time units",
                            NULL, units->second, &units->second, NULL);
  CHKERRQ(ierr);
  units->second = fabs(units->second);

  ierr = PetscOptionsScalar("-units_kilogram", "1 kilogram in scaled mass units",
                            NULL, units->kilogram, &units->kilogram, NULL);
  CHKERRQ(ierr);
  units->kilogram = fabs(units->kilogram);

  ierr = PetscOptionsEnd(); CHKERRQ(ierr); // End of setting Physics

  // Check for all required options to be set
  if (!nu_flag) {
    SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP, "-nu option needed");
  }
  if (!Young_flag) {
    SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP, "-E option needed");
  }

  // Define derived units
  units->Pascal = units->kilogram / (units->meter * PetscSqr(units->second));

  // Scale E to Pa
  phys->E *= units->Pascal;

  PetscFunctionReturn(0);
};


/**
 * \file gps_single.c Implements GPS single point solution using Least Squares
 * \author Duncan Greer
 * $Id: gps_single.c 3579 2010-06-26 10:57:31Z greerd $
 */

#include <stdio.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <time.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_linalg.h>
#include "../../novatelstore/src/novatel.h"
#include "gps.h"
#include "../../novatelstore/src/shmdef.h"

#include "matrix.h"

/** Controls whether or not the program should keep running. */
int iShutdown = 0;

#define MODULE_NAME "[GPS_SINGLE]"

#define log(string) 	fprintf(stdout,"%s %s\n", MODULE_NAME, string)
#define err(string)	fprintf(stderr,"%s ERROR: %s\n", MODULE_NAME, string)

#define ELEVATION_CUTOFF_RAD (10.0 * DEG2RAD)

/**
	\function CalculateDOP
	Calculates the Dilution of Precision based on the provided G Matrix.
	\author Duncan Greer and Troy Bruggemann

	Note that the G-Matrix must be provided already transformed to
	the horizontal plane.
*/
void CalculateDOP(gsl_matrix *G, double *DOP);

/**
 *	\fn HandleSignal
 *	\brief Handle caught signals
 *
 * 	This function sets a flag (iShutdown) to shut the program down.
 */
void HandleSignal(int signal)
{
	fprintf(stdout,"Got Signal: %d\n",signal);
	iShutdown = 1;
}

/**
 * 	\fn Usage
 *	\brief Print Usage Information
 */
void Usage(char *arg)
{

	/* options */
	/*
	    b = baudrate
	    p = port
	    l = logging
	    f = log filename
	    t = enable tcp server
	*/
	fprintf(stdout,"Usage: %s <options>\n",arg);
	fprintf(stdout,"    -l Log results to file (OFF)\n");
	fprintf(stdout,"    -f log filename\n");
	fprintf(stdout,"    -L Use specified log file path (/data/gps)\n");

}

int main (int argc, char **argv)
{
	gsl_matrix *G;
	gsl_matrix *G_LTP;
//	gsl_matrix *GT;
//	gsl_matrix *tempNN;
//	gsl_matrix *V;
//	gsl_vector *S;
//	gsl_matrix *U;
	gsl_vector *W;
	gsl_vector *y;
	gsl_vector *delta_y;
	gsl_vector *x;
	gsl_vector *delta_x;
	gsl_vector *x_error;
	gsl_vector *x_true;
	gsl_vector *temp1;

	gsl_multifit_linear_workspace *gsl_workspace;
	gsl_vector *SVPos[MAX_SV];
	double SVPos_calc[4];
	double dSSE;
	gsl_matrix *SolutionCovariance;
	int iReturnValue = 0;

	int iNumberStates = 4;
	int iNumberMeasurements = 9;
	int iNumberValidEphemeris = 0;
	int iNumberValidObservations = 0;
	int iIndex;
	GPSTime gtObservationTime;
	unsigned short usPRN;
	int iIteration;

	int iUseSV[NOVATEL_MAXPRNS];

	double user_x,user_y,user_z,user_t = 0.0;
	double sat_x,sat_y,sat_z,sat_t;
//	double sat_x_calc,sat_y_calc,sat_z_calc,sat_t_calc;
	double dTemp;

	double range;
	double delta_pr_omegaedot;

	double ecef[3];
	double llh[3];
	double TECEF2ENU[3][3];
	gsl_matrix *T_ECEF2NED_44;
	double NEDError[3];
	double ECEFError[3];
	double UserPos[4];
	double ION_Alpha[4];
	double ION_Beta[4];
	double IonoDelay[NOVATEL_MAXCHANNELS];
	double TropoDelay[NOVATEL_MAXCHANNELS];
	double SV_Azimuth[NOVATEL_MAXCHANNELS];
	double SV_Elevation[NOVATEL_MAXCHANNELS];

	double dTransmissionTime;

	GPSEPHEM  GPSEphemData[NOVATEL_MAXPRNS];
	RANGE GPSRangeData;

	double GDOP,PDOP;
	double DOPS[5];
	double limit;

	int iUseSHM = 1;
	int iSHM_FID;

	int iRow,iCol;

	shm_struct *shm = NULL;

	unsigned long ulSignalType = 0;

	char copt;

	/* log files */
	FILE *fpLogFile = NULL;    /* pointer to log file stream */
	char azcLogFileName[256];  /* log file name */
	char azcLogFilePath[255];
	char azcLogFileNameWithPath[1024];

	struct tm *timestruct;
	time_t tCurrentTime = 0;
	time_t tLogFileStart = 0;

	/* options flags */
	struct
	{
		int iLogResults;
		int iCustomLogFile;
	} options;

	// default options
	options.iLogResults = 0;
	options.iCustomLogFile = 0;

	/* setup signal handlers */
	signal(SIGHUP, HandleSignal);
	signal(SIGTERM, HandleSignal);
	signal(SIGKILL, HandleSignal);
	signal(SIGALRM, HandleSignal);
	signal(SIGINT, HandleSignal);

	/* get the current time to use as a timestamp in the log file name */
	tCurrentTime = time(NULL);
	timestruct = gmtime(&tCurrentTime);

	/* setting default log file path */
	sprintf(azcLogFilePath,"%s","/data/gps");

	/* read input parameters */
	while((copt = getopt(argc,argv,"lf:L:")) && copt != -1)
	{
	  switch(copt)
	  {
	   case 'l':
		/* log results to file */
		options.iLogResults = 1;
		log("Logging enabled");
		strftime(azcLogFileName,sizeof(azcLogFileName),"gps_single_%g%m%d%H%M%S.csv",timestruct);
		break;

	  case 'f':
		/* set the log file name */
		sprintf(azcLogFileName,"%s",optarg);
		options.iCustomLogFile = 1; /* since we're using a user specified log file name, we dont want to recycle it every hour */
		break;

	  case 'L':
	  	/* setting log file path */
	  	sprintf(azcLogFilePath,"%s",optarg);

	  	fprintf(stdout,"%s Setting log file path to %s",MODULE_NAME,azcLogFilePath);
	  	break;
	  case '?':
		Usage(argv[0]);
		return 0;
		break;
	  }
	}

	for(iIndex=0;iIndex<NOVATEL_MAXPRNS;iIndex++)
	{
		iUseSV[iIndex] = 0;
	}

	fprintf(stdout,"Allocating Matrices\n");
	x = gsl_vector_alloc(iNumberStates);
	delta_x = gsl_vector_alloc(iNumberStates);
	x_true = gsl_vector_alloc(iNumberStates);
	x_error = gsl_vector_alloc(iNumberStates);
	SolutionCovariance = gsl_matrix_alloc(iNumberStates,iNumberStates);

	temp1 = gsl_vector_alloc(3);

	for(iIndex=0;iIndex<MAX_SV;iIndex++)
	{
		SVPos[iIndex] = gsl_vector_alloc(4);
	}

	T_ECEF2NED_44 = gsl_matrix_alloc(4,4);

       	/* setup shared memory */
	if(iUseSHM == 1)
	{
	  iSHM_FID = shm_open("/novatel0",O_RDWR,0777);
	  if(iSHM_FID == -1)
	  {
		perror("shm_open: ");
		iUseSHM = 0;
 	  } else
	  {
	     	/* memory map the shm object */
	     	shm = mmap(0,sizeof(*shm),PROT_READ,MAP_SHARED,iSHM_FID,0);
	     	if(shm == MAP_FAILED)
	     	{
		   perror("shm mmap: ");
		   iUseSHM = 0;
	     	} else
		{
		  fprintf(stdout,"%s SHM Address Is: 0x%08x\n",MODULE_NAME,(unsigned int)shm);
		}
	  }
	}

	/* setup time */
	gtObservationTime.usGPSWeek = 1416;
	gtObservationTime.fGPSSecondOfWeek = 135900.0;

	/* setup measurements */
/*	GPSRangeData.usPRN[0] = 10;
	GPSRangeData.usPRN[1] = 12;
	GPSRangeData.usPRN[2] = 30;
	GPSRangeData.usPRN[3] = 29;
	GPSRangeData.usPRN[4] = 7;
	GPSRangeData.usPRN[5] = 26;
	GPSRangeData.usPRN[6] = 22;
	GPSRangeData.usPRN[7] = 24;
	GPSRangeData.dPseudorange[0] = 23677691.85548;
	GPSRangeData.dPseudorange[1] = 24609189.10747;
	GPSRangeData.dPseudorange[2] = 22288602.64148;
	GPSRangeData.dPseudorange[3] = 22641003.92448;
	GPSRangeData.dPseudorange[4] = 20042814.45649;
	GPSRangeData.dPseudorange[5] = 23024522.23148;
	GPSRangeData.dPseudorange[6] = 24837761.16146;
	GPSRangeData.dPseudorange[7] = 21114887.48349;
*/
	/* setup sat positions */

	/* estimated position - starting point */
	gsl_vector_set(x,0,-5046773.0);
	gsl_vector_set(x,1, 2568446.0);
	gsl_vector_set(x,2,-2925289.0);
	gsl_vector_set(x,3, 0.0);

	llh[0] = -0.47;
	llh[1] = 2.53;
	llh[2] = 0.0;

	//gsl_vector_fprintf(stdout,x,"%10.3f");

	/* true position */
	gsl_vector_set(x_true,0,-5053034.84 );
	gsl_vector_set(x_true,1, 2562951.08);
	gsl_vector_set(x_true,2,-2919245.94);
	gsl_vector_set(x_true,3,0.0);

/*
	// PRN 10
	gsl_vector_set(SVPos[10],0,-11106779.576);
	gsl_vector_set(SVPos[10],1,-10162086.179);
	gsl_vector_set(SVPos[10],2,-21984106.076);
	gsl_vector_set(SVPos[10],3, 98.034009e-6*GPS_SPEEDOFLIGHT);

	// PRN 12
	gsl_vector_set(SVPos[12],0,-20114520.008);
	gsl_vector_set(SVPos[12],1,  6748657.642);
	gsl_vector_set(SVPos[12],2, 16058860.089);
	gsl_vector_set(SVPos[12],3,-47.814630e-6*GPS_SPEEDOFLIGHT);

	// PRN 30
	gsl_vector_set(SVPos[30],0, -19111202.845);
	gsl_vector_set(SVPos[30],1, 17011909.480);
	gsl_vector_set(SVPos[30],2, 6599201.305);
	gsl_vector_set(SVPos[30],3, 26.513082e-6*GPS_SPEEDOFLIGHT);

	// PRN 29
	gsl_vector_set(SVPos[29],0, -22931680.210);
	gsl_vector_set(SVPos[29],1, -10007127.790);
	gsl_vector_set(SVPos[29],2,  -9208106.350);
	gsl_vector_set(SVPos[29],3, 358.103982e-6*GPS_SPEEDOFLIGHT);

	// PRN 7
	gsl_vector_set(SVPos[7],0, -18000552.275);
	gsl_vector_set(SVPos[7],1,  10196913.772);
	gsl_vector_set(SVPos[7],2, -16363130.228);
	gsl_vector_set(SVPos[7],3, 402.849928e-6*GPS_SPEEDOFLIGHT);

	// PRN 26
	gsl_vector_set(SVPos[26],0, -24783876.733);
	gsl_vector_set(SVPos[26],1,  -8910991.974);
	gsl_vector_set(SVPos[26],2,  -5742918.813);
	gsl_vector_set(SVPos[26],3, -62.236139e-6*GPS_SPEEDOFLIGHT);

	// PRN 22
	gsl_vector_set(SVPos[22],0, -8616964.914);
	gsl_vector_set(SVPos[22],1, 22105308.026);
	gsl_vector_set(SVPos[22],2, 12074730.766);
	gsl_vector_set(SVPos[22],3, 169.425839e-6*GPS_SPEEDOFLIGHT);

	// PRN 24
	gsl_vector_set(SVPos[24],0, -15110809.416);
	gsl_vector_set(SVPos[24],1,   2514877.198);
	gsl_vector_set(SVPos[24],2, -21514889.442);
	gsl_vector_set(SVPos[24],3, 80.656626e-6*GPS_SPEEDOFLIGHT);

	*/

	/* open log file */
	if(options.iLogResults == 1)
	{
		sprintf(azcLogFileNameWithPath,"%s/%s",azcLogFilePath,azcLogFileName);

		if((fpLogFile = fopen(azcLogFileNameWithPath,"a")) == NULL)
		{
			fprintf(stderr,"Error opening %s for writing: %s\n",azcLogFileNameWithPath,strerror(errno));

			return -1;
		} else
		{
			tLogFileStart = tCurrentTime;
			fprintf(stdout,"Using log file: %s\n",azcLogFileNameWithPath);
		}

	}

  while(iShutdown == 0)
  {

	/* if log file has been opend for > 1 hour, start a new one */
	if(options.iLogResults == 1 && options.iCustomLogFile == 0)
	{
		tCurrentTime = time(NULL);
		if(difftime(tCurrentTime,tLogFileStart) > 3600.0)
		{
			/* start a new log file */
			fclose(fpLogFile);

			timestruct = gmtime(&tCurrentTime);
			strftime(azcLogFileName,sizeof(azcLogFileName),"gps_single_%g%m%d%H%M%S.csv",timestruct);

			sprintf(azcLogFileNameWithPath,"%s/%s",azcLogFilePath,azcLogFileName);

			if((fpLogFile = fopen(azcLogFileNameWithPath,"a")) == NULL)
			{
				fprintf(stderr,"Error opening %s for writing: %s\n",azcLogFileNameWithPath,strerror(errno));
				options.iLogResults = 0;
			} else
			{
				tLogFileStart = tCurrentTime;
				fprintf(stdout,"%s Started New Logfile: %s\n",MODULE_NAME,azcLogFileNameWithPath);

			}
		}

	}

	/* TODO:  replace this sleep with a semaphore to wait for new data */
  	sleep(1);

	fprintf(stdout,"====== Single-Point Solution ======\n");
	/* retrive the current data from shm */
	if(iUseSHM == 1)
	{
	  memcpy(&GPSEphemData,&shm->CurrentGPSEPHEM,sizeof(GPSEPHEM)*NOVATEL_MAXPRNS);

	  // get the current observation time to compare with ephemeris
	  memcpy(&gtObservationTime,&shm->CurrentRANGE.gtTimeStamp,sizeof(GPSTime));

	  /* ensure we have ephemeris for all measurements, otherwise exclude */
	  iNumberValidEphemeris = 0;
	  for(iIndex=0;iIndex<NOVATEL_MAXPRNS;iIndex++)
	  {
	  	if((GPSEphemData[iIndex].ulPRN != 0) &&
		   (GPSEphemData[iIndex].ulHealth == 0) &&
		   (GPSEphemData[iIndex].dTOE < (gtObservationTime.fGPSSecondOfWeek+7500.0)) &&
		   (GPSEphemData[iIndex].dTOE > (gtObservationTime.fGPSSecondOfWeek-7500.0))
	  	)
	  	{
	  	  iUseSV[iIndex] = 1;
	  	  iNumberValidEphemeris++;
//	  	  fprintf(stdout,"SV%d ",iIndex);
	  	} else
	  	{
	  	  iUseSV[iIndex] = 0;
	  	}
	  }

	  /* loop through measurements and see
	     if we have valid ephem for each */

	  memcpy(&GPSRangeData.gtTimeStamp,&shm->CurrentRANGE.gtTimeStamp,sizeof(GPSTime));
	  iNumberValidObservations = 0;

	  for(iIndex=0;iIndex<shm->CurrentRANGE.lNumberObservations;iIndex++)
	  {
	  	if(iUseSV[shm->CurrentRANGE.usPRN[iIndex]] == 1)
	  	{
	  		ulSignalType = shm->CurrentRANGE.ulTrackingStatus[iIndex] & 0x3E00000;

	  		//  check that we're only getting hte L1 signal
	  		if (ulSignalType == 0)
	  		{
	  			// use current range data for now
	  			memcpy(&GPSRangeData.usPRN[iNumberValidObservations],
				        &shm->CurrentRANGE.usPRN[iIndex],sizeof(unsigned short));
				memcpy(&GPSRangeData.dPseudorange[iNumberValidObservations],
					&shm->CurrentRANGE.dPseudorange[iIndex], sizeof(double));

					iNumberValidObservations++;

	  		} else
	  		{

				// not an L1 obs
				//fprintf(stdout,"Value of Signal Type for PRN%02d was 0x%0x\n",
				//	shm->CurrentRANGE.usPRN[iIndex],
				//	ulSignalType);
	  		}
	  	} else
	  	{
	  	  fprintf(stdout,"NE: %d; ",shm->CurrentRANGE.usPRN[iIndex]);
	  	}
	  }

	  fprintf(stdout,"Obs: %d; \n",iNumberValidObservations);
	  GPSRangeData.lNumberObservations = iNumberValidObservations;

	  iNumberMeasurements = GPSRangeData.lNumberObservations;

	  if(iNumberMeasurements < 4)
	  {
		fprintf(stdout,"Less than 4 measurements\n");
		continue;
	  }

	/*
		  if(iNumberMeasurements > 9)
		  {
			 iNumberMeasurements = 9;
		  }

		  if(iNumberMeasurements < 9)
		  {
			err("Less than 9 measurements!");
		  }
	*/
		G = gsl_matrix_alloc(iNumberMeasurements,iNumberStates);
		G_LTP = gsl_matrix_alloc(iNumberMeasurements,iNumberStates);
		y = gsl_vector_alloc(iNumberMeasurements);
		delta_y = gsl_vector_alloc(iNumberMeasurements);
		W = gsl_vector_alloc(iNumberMeasurements);
		gsl_workspace = gsl_multifit_linear_alloc (iNumberMeasurements,iNumberStates);

		/* calculate G-matrix */
		gsl_matrix_set_zero (G);
		//gsl_matrix_fprintf(stdout,G,"%5.3f");

		/* calculate W-matrix */
		gsl_vector_set_all(W,1.0);

	} else
	{
		log("No Shared Memory");

	}

		for(iIteration=0;iIteration<20;iIteration++)
		{
		//	fprintf(stdout,"\n===== ITERATION %d =====\n",iIteration+1);

			user_x = gsl_vector_get(x,0);
			user_y = gsl_vector_get(x,1);
			user_z = gsl_vector_get(x,2);
			user_t = gsl_vector_get(x,3);

			UserPos[0] = user_x;
			UserPos[1] = user_y;
			UserPos[2] = user_z;
			UserPos[3] = user_t;

			ION_Alpha[0] = shm->CurrentIONUTC.a0;
			ION_Alpha[1] = shm->CurrentIONUTC.a1;
			ION_Alpha[2] = shm->CurrentIONUTC.a2;
			ION_Alpha[3] = shm->CurrentIONUTC.a3;

			ION_Beta[0] = shm->CurrentIONUTC.b0;
			ION_Beta[1] = shm->CurrentIONUTC.b1;
			ION_Beta[2] = shm->CurrentIONUTC.b2;
			ION_Beta[3] = shm->CurrentIONUTC.b3;

	//		fprintf(stdout,"User: [%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f]\n",user_x,user_y,user_z,user_t);

			for (iIndex=0;iIndex<iNumberMeasurements;iIndex++)
			{

				dTransmissionTime = (double)gtObservationTime.fGPSSecondOfWeek - GPSRangeData.dPseudorange[iIndex]/GPS_SPEEDOFLIGHT;

				GPSOrbitPropagator(dTransmissionTime,
						GPSRangeData.usPRN[iIndex],
						&GPSEphemData[GPSRangeData.usPRN[iIndex]],
						7500.0,
						SVPos_calc);

				usPRN = GPSRangeData.usPRN[iIndex];

				sat_x = SVPos_calc[0];
				sat_y = SVPos_calc[1];
				sat_z = SVPos_calc[2];
				sat_t = SVPos_calc[3]*GPS_SPEEDOFLIGHT;

				// find the range
				//gsl_vector_memcpy(temp1,SVPos[usPRN]);
				//gsl_vector_sub(temp1,x);
				gsl_vector_set(temp1,0,sat_x - user_x);
				gsl_vector_set(temp1,1,sat_y - user_y);
				gsl_vector_set(temp1,2,sat_z - user_z);

				range = gsl_blas_dnrm2(temp1);

				gsl_matrix_set(G,iIndex,0,-(sat_x - user_x)/range);
				gsl_matrix_set(G,iIndex,1,-(sat_y - user_y)/range);
				gsl_matrix_set(G,iIndex,2,-(sat_z - user_z)/range);
				gsl_matrix_set(G,iIndex,3,1.0);

				gsl_vector_set(y,iIndex,GPSRangeData.dPseudorange[iIndex]);

				// calculate earth rotation correction
				delta_pr_omegaedot = -(GPS_OMEGAEDOT/GPS_SPEEDOFLIGHT) * (sat_x*user_y - sat_y*user_x);

				// calculate iono correction
				ICD200_IonoModel(gtObservationTime,
						UserPos,
						SVPos_calc,
						ION_Alpha,
						ION_Beta,
						&IonoDelay[iIndex],
						&SV_Azimuth[iIndex],
						&SV_Elevation[iIndex]);

				// examine sv elevations - if less than cutoff, set corresponding row of 'G' to zeros
				if (SV_Elevation[iIndex] < ELEVATION_CUTOFF_RAD)
				{
					gsl_matrix_set(G,iIndex,0,0.0);
					gsl_matrix_set(G,iIndex,1,0.0);
					gsl_matrix_set(G,iIndex,2,0.0);
					gsl_matrix_set(G,iIndex,3,0.0);
					fprintf(stdout,"Ignoring PRN%02d due low elevation (%3.1fdeg)\n",
						GPSRangeData.usPRN[iIndex],SV_Elevation[iIndex]*RAD2DEG);

					TropoDelay[iIndex] = 0.0;
					gsl_vector_set(delta_y,iIndex,0.0);
				} else
				{

					TropoDelay[iIndex] = TropoDelayModel(SV_Elevation[iIndex],llh[2]);

					gsl_vector_set(delta_y,iIndex,GPSRangeData.dPseudorange[iIndex]-range-(user_t-sat_t)+delta_pr_omegaedot-IonoDelay[iIndex]-TropoDelay[iIndex]);
				}
	//			fprintf(stdout,"Sat %d:\t[%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f] range=%f, delta_pr=%f\n",usPRN,sat_x,sat_y,sat_z,sat_t,range,delta_pr_omegaedot);
	//			fprintf(stdout,"Sat %d:\t[%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f]\n",usPRN,sat_x,sat_y,sat_z,sat_t);

			}

	//		fprintf(stdout,"delta_y: \n");
	//		gsl_vector_fprintf(stdout,delta_y,"%10.3f");

//			fprintf(stdout,"G-Matrix: \n");
//			for (iIndex=0;iIndex<iNumberMeasurements;iIndex++)
//			{
//				fprintf(stdout,"[%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f]\n",
//					gsl_matrix_get(G,iIndex,0),
//					gsl_matrix_get(G,iIndex,1),
//					gsl_matrix_get(G,iIndex,2),
//					gsl_matrix_get(G,iIndex,3));
//
//			}

			/* weight measurements according to elevation */
//			for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
//			{
//			  gsl_vector_set(W,iIndex,SV_Elevation[iIndex]);
//			}

			//gsl_vector_fprintf(stdout,W,"%f");

			iReturnValue = gsl_multifit_wlinear(G,
							W,
							delta_y,
							delta_x,
							SolutionCovariance,
							&dSSE,
							gsl_workspace);

			/*iReturnValue = gsl_multifit_linear(G,
							delta_y,
							delta_x,
							SolutionCovariance,
							&dSSE,
							gsl_workspace);
			*/
			gsl_vector_add(x,delta_x);

		//	fprintf(stdout,"Least-squares fit returned value: %d; RSS=%f\n",iReturnValue,sqrt(dSSE));
		//	fprintf(stdout,"delta_x: \n");
		//	gsl_vector_fprintf(stdout,delta_x,"%20.12f");

			gsl_vector_memcpy(x_error,x);
			gsl_vector_sub(x_error,x_true);

			/* test if we should break out */
			limit = gsl_blas_dnrm2(delta_x);
			if (limit < 1e-8)
			{
				fprintf(stdout,"%f:Least-Squares solved in %d iterations; RSS=%f\n",
					gtObservationTime.fGPSSecondOfWeek,iIteration+1,sqrt(dSSE));

				/* convert to LLH */
				ecef[0] = gsl_vector_get(x,0);
				ecef[1] = gsl_vector_get(x,1);
				ecef[2] = gsl_vector_get(x,2);

				WGS84_ECEF2LLH(ecef, llh);
				T_ECEF2ENU(llh[0], llh[1], (double **)TECEF2ENU);

	//			fprintf(stdout,"[%f %f %f]\n",TECEF2ENU[0][0],TECEF2ENU[0][1],TECEF2ENU[0][2]);
	//			fprintf(stdout,"[%f %f %f]\n",TECEF2ENU[1][0],TECEF2ENU[1][1],TECEF2ENU[1][2]);
	//			fprintf(stdout,"[%f %f %f]\n",TECEF2ENU[2][0],TECEF2ENU[2][1],TECEF2ENU[2][2]);

				ECEFError[0] = gsl_vector_get(x_error,0);
				ECEFError[1] = gsl_vector_get(x_error,1);
				ECEFError[2] = gsl_vector_get(x_error,2);

				MatMul331(TECEF2ENU, ECEFError, NEDError);

				/* must swap ENU to NED */
				dTemp = NEDError[0];
				NEDError[0] = NEDError[1];
				NEDError[1] = dTemp;
				NEDError[2] = -NEDError[2];

				//fprintf(stdout,"X error: \n");
				//gsl_vector_fprintf(stdout,x_error,"%10.5f m");

				//fprintf(stdout,"X: \n");
				//gsl_vector_fprintf(stdout,x,"%10.5f m");

				/* calculate DOPS */
				for(iRow=0;iRow<3;iRow++)
				{
				  for(iCol=0;iCol<3;iCol++)
				  {
					// note that TECEF2ENU must be transposed
					gsl_matrix_set(T_ECEF2NED_44,
						iRow,iCol,TECEF2ENU[iCol][iRow]);
				  }
				  gsl_matrix_set(T_ECEF2NED_44,iRow,3,0.0);
				}
				for(iCol=0;iCol<3;iCol++)
				{
				  gsl_matrix_set(T_ECEF2NED_44,3,iCol,0.0);
				}
				gsl_matrix_set(T_ECEF2NED_44,3,3,1.0);

				gsl_matrix_set_zero(G_LTP);
				gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,
					1.0, G, T_ECEF2NED_44, 1.0, G_LTP);

				CalculateDOP(G_LTP,DOPS);
				GDOP = DOPS[0];
				PDOP = DOPS[1];

				/* write results to shared memory */
//				memcpy(&shm->dDOPS[0],&DOPS[0],sizeof(double));
//				memcpy(shm->dLLH_LSQ,llh,sizeof(double)*3);
//				memcpy(shm->dECEF_LSQ,ecef,sizeof(double)*3);
//				shm->dRxClock_LSQ = gsl_vector_get(x,3);

				fprintf(stdout,"Lat: %f, Long: %f, Height: %f\n",
					llh[0]*RAD2DEG,llh[1]*RAD2DEG,llh[2]);
				fprintf(stdout,"GDOP=%f, PDOP=%f, HDOP=%f, VDOP=%f, TDOP=%f\n",
					GDOP,PDOP,DOPS[2],DOPS[3],DOPS[4]);
				fprintf(stdout,"NED Error: %fm %fm %fm\n",NEDError[0],
					NEDError[1],
					NEDError[2]);

				fprintf(stdout,"SV: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%d ",GPSRangeData.usPRN[iIndex]);
				}
				fprintf(stdout,"\n");

				fprintf(stdout,"Iono: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1fm ",IonoDelay[iIndex]);
				}
				fprintf(stdout,"\n");

				fprintf(stdout,"Tropo: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1fm ",TropoDelay[iIndex]);
				}
				fprintf(stdout,"\n");

				fprintf(stdout,"Elevation: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1f ",SV_Elevation[iIndex]*RAD2DEG);
				}
				fprintf(stdout,"\n");
				fprintf(stdout,"Azimuth: ");
				for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
				{
				  fprintf(stdout,"%3.1f ",SV_Azimuth[iIndex]*RAD2DEG);
				}
				fprintf(stdout,"\n");

				// check if we should log to disk
				if(options.iLogResults == 1 && fpLogFile != NULL)
				{

					fprintf(fpLogFile, "%d,%f,%d,%lf,%d,%lf,%lf,%lf,%.10lf,%.10lf,%lf,%f,%f,%f,%f,%f\n",
						gtObservationTime.usGPSWeek,
						gtObservationTime.fGPSSecondOfWeek,
						iIteration+1,
						sqrt(dSSE),
						iNumberMeasurements,
						ecef[0],
						ecef[1],
						ecef[2],
						llh[0],
						llh[1],
						llh[2],
						DOPS[0],
						DOPS[1],
						DOPS[2],
						DOPS[3],
						DOPS[4]
					);
					fflush(fpLogFile);
				}

				break;

			}

		}

		if (iIteration == 20)
		{
			fprintf(stderr,"Iterated 20 times with no solution (limit=%f)", limit);
		}

		gsl_matrix_free(G);
		gsl_matrix_free(G_LTP);
		gsl_vector_free(W);
		gsl_vector_free(y);
		gsl_vector_free(delta_y);
		gsl_multifit_linear_free(gsl_workspace);
	}

	fclose(fpLogFile);

	fprintf(stdout,"Freeing Matrices\n");

	gsl_vector_free(x);
	gsl_vector_free(delta_x);
	gsl_vector_free(x_true);
	gsl_vector_free(x_error);

	gsl_vector_free(temp1);
	gsl_matrix_free(SolutionCovariance);

	gsl_matrix_free(T_ECEF2NED_44);

	for(iIndex=0;iIndex<MAX_SV;iIndex++)
	{
		gsl_vector_free(SVPos[iIndex]);
	}

	return 0;
}

void CalculateDOP(gsl_matrix *G, double *DOP)
{
	gsl_matrix *GT;
	gsl_matrix *tempNN;
	gsl_matrix *V;
	gsl_vector *S;
	gsl_vector *Sinv;
	gsl_matrix *SinvMat;
	gsl_matrix *A;  // = inv(GT*G);
	gsl_vector *SVDWork;
	gsl_matrix *U;

	int iReturnValue,iIndex;
	int iNumberMeasurements,iNumberStates;

	double var_x,var_y,var_z,var_dt;

	iNumberMeasurements = G->size1;
	iNumberStates = G->size2;

	GT = gsl_matrix_alloc(iNumberStates,iNumberMeasurements);
	tempNN = gsl_matrix_alloc(iNumberStates,iNumberStates);
	V = gsl_matrix_alloc(iNumberStates,iNumberStates);
	S = gsl_vector_alloc(iNumberStates);
	Sinv = gsl_vector_alloc(iNumberStates);
	SinvMat = gsl_matrix_alloc(iNumberStates,iNumberStates);
	A = gsl_matrix_alloc(iNumberStates,iNumberStates);
	SVDWork = gsl_vector_alloc(iNumberStates);
	U = gsl_matrix_alloc(iNumberStates,iNumberStates);

	// calculate DOP
	gsl_matrix_transpose_memcpy(GT,G);
	gsl_matrix_set_zero(tempNN);
	gsl_blas_dgemm (CblasNoTrans,CblasNoTrans, 1.0, GT, G, 1.0, tempNN);

	// take the SVD of GT*T (stored in TempNN) to find inverse - tempNN now contains 'U'
	iReturnValue =  gsl_linalg_SV_decomp (tempNN,V,S,SVDWork);

	// get element wise inverse of S
	gsl_vector_set_all(Sinv,1.0);
	gsl_vector_div(Sinv, S);

	gsl_matrix_set_zero (SinvMat);
	for(iIndex=0;iIndex<iNumberStates;iIndex++)
	{
		gsl_matrix_set(SinvMat,iIndex,iIndex,gsl_vector_get(Sinv,iIndex));
	}

	// inv(S)*UT
	gsl_matrix_memcpy(U,tempNN);
	gsl_matrix_set_zero (tempNN);
	gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, SinvMat, U, 1.0, tempNN);

	// A = V * inv(S) * UT
	gsl_matrix_set_zero(A);
	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, V, tempNN, 1.0, A);

	var_x = gsl_matrix_get(A,0,0);
	var_y = gsl_matrix_get(A,1,1);
	var_z = gsl_matrix_get(A,2,2);
	var_dt = gsl_matrix_get(A,3,3);

	DOP[0] = sqrt(var_x+var_y+var_z+var_dt);
	DOP[1] = sqrt(var_x+var_y+var_z);
	DOP[2] = sqrt(var_x+var_y);
	DOP[3] = sqrt(var_z);
	DOP[4] = sqrt(var_dt);

	gsl_matrix_free(GT);
	gsl_matrix_free(tempNN);

	gsl_matrix_free(A);
	gsl_matrix_free(V);
	gsl_vector_free(S);
	gsl_vector_free(Sinv);
	gsl_matrix_free(U);
	gsl_vector_free(SVDWork);
	gsl_matrix_free(SinvMat);

}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "compearth.h"
#include "cmopad.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_cblas.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <cblas.h>
#endif

struct mt_struct
{
    double mt[6];
    double tax[3];
    double bax[3];
    double pax[3];
    double sdr1[3]; //strike dip rake; fault plane 2
    double sdr2[3]; //strike dip rake; fault plane 1
    double mag;
    double exp;
};

/*
int compearth_standardDecomposition(const int nmt,
                                    const double *__restrict__ M,
                                    enum compearthCoordSystem_enum basis,
                                    double *__restrict__ M0,
                                    double *__restrict__ Mw,
                                    double *__restrict__ fp1,
                                    double *__restrict__ fp2,
                                    double *__restrict__ pAxis,
                                    double *__restrict__ bAxis,
                                    double *__restrict__ tAxis,
                                    double *__restrict__ isoPct,
                                    double *__restrict__ devPct,
                                    double *__restrict__ dcPct,
                                    double *__restrict__ clvdPct);
*/
int decompose(const int nmt, const double mtUSE[6]);

int decompose(const int nmt, const double mtUSE[6])
{
    //double K[3], N[3], S[3], lam[3], U[9], Uuse[9], b[3], p[3], t[3],
    //double gamma, delta, M0, kappa, theta, sigma, thetadc,
    //       k2, d2, s2;
    struct cmopad_struct src;
    const char *fcnm = "decompose\0";
    enum cmopad_basis_enum cin, cloc;
    double M[3][3], pax[3], bax[3], tax[3];
    int i, ierr;
    const int iverb = 0;
/*
    //------------------------------------------------------------------------//
    //                       Do the cMoPaD decomposition                      //
    //------------------------------------------------------------------------//
    // Copy moment tensor
    M[0][0] = mtUSE[0];           //Mrr
    M[1][1] = mtUSE[1];           //Mtt
    M[2][2] = mtUSE[2];           //Mpp
    M[0][1] = M[1][0] = mtUSE[3]; //Mrt
    M[0][2] = M[2][0] = mtUSE[4]; //Mrp
    M[1][2] = M[2][1] = mtUSE[5]; //Mtp
    // Mopad works in North, East, Down frame but inversion is Up, South, East
    cin = USE;
    cloc = NED;
    cmopad_basis_transformMatrixM33(M, cin, cloc); //USE -> NED
    // Compute the isotropic, CLVD, DC decomposition
    ierr = cmopad_standardDecomposition(M, &src);
    if (ierr != 0)
    {
        printf("%s: Error in decomposition!\n", fcnm);
        return EXIT_FAILURE;
    }
    // Compute the princple axes with corresponding strikes, dips, and rakes
    ierr = cmopad_MT2PrincipalAxisSystem(iverb, &src);
    if (ierr != 0)
    {
        printf("%s: Error computing principal axis\n",fcnm);
        return EXIT_FAILURE;
    }
    // Compute the pressure, null, and, tension principal axes as len,az,plunge
    cin = NED; //MoPaD is in north, east, down
    ierr = cmopad_Eigenvector2PrincipalAxis(cin, src.eig_pnt[0],
                                            src.p_axis,    pax);
    if (ierr != 0)
    {
        printf("%s: Error converting pax\n",fcnm);
        return EXIT_FAILURE;
    }
    ierr = cmopad_Eigenvector2PrincipalAxis(cin, src.eig_pnt[1],
                                            src.null_axis, bax);
    if (ierr != 0){
        printf("%s: Error converting bax\n",fcnm);
        return EXIT_FAILURE;
    }
    ierr = cmopad_Eigenvector2PrincipalAxis(cin, src.eig_pnt[2],
                                            src.t_axis,    tax);
    if (ierr != 0)
    {
        printf("%s: Error converting tax\n",fcnm);
        return EXIT_FAILURE;
    }
*/
    //printf("%f %f %f %f %f %f\n",
    //       mtUSE[0], mtUSE[1], mtUSE[2], mtUSE[3], mtUSE[4], mtUSE[5]);
    //ierr = compearth_CMT2TT(1, mtUSE, false,
    //                        &gamma, &delta, &M0,
    //                        &kappa, &theta, &sigma,
    //                        K, N, S, &thetadc,
    //                        lam, U);
    //if (ierr != 0)
    //{
    //    fprintf(stderr, "%s: CMT2TT failed\n", __func__);
    //    return EXIT_FAILURE;
    //}
    /*
    // Need to switch from SEU to USE.  This requires computing
    // R U inv(R) where:
    //  R = [0 0 1
    //       1 0 0
    //       0 1 0]
    // Carrying through the multiplication yields the permutation
    //  U_{use} = [U_{33} U_{31} U_{32}
    //             U_{13} U_{11} U_{12}
    //             U_{23} U_{21} U_{22}]
    //
    // Permute the column major matrix
    Uuse[0] = U[8];
    Uuse[1] = U[6];
    Uuse[2] = U[7];

    Uuse[3] = U[2];
    Uuse[4] = U[0];
    Uuse[5] = U[1];

    Uuse[6] = U[5];
    Uuse[7] = U[3];
    Uuse[8] = U[4];
printf("%f %f %f\n%f %f %f\n%f %f %f\n",
       U[0], U[3], U[6],
       U[1], U[4], U[7],
       U[2], U[5], U[8]);
*/
    //ierr = compearth_U2pa(1, U,
    //                      &p[2], &p[1], &b[2], &b[1], &t[2], &t[1]);
    //if (ierr != 0)
    //{
    //    fprintf(stderr, "%s: Error converting u to plunge/azimuth\n",
    //            __func__);
    //    return EXIT_FAILURE;
    //}
    //// Fill in the eigenvalues
    //p[0] = lam[0];
    //b[0] = lam[1];
    //t[0] = lam[2];
    //printf("p: %e %f %f\n", p[0], p[1], p[2]);
    //printf("b: %e %f %f\n", b[0], b[1], b[2]);
    //printf("t: %e %f %f\n", t[0] ,t[1], t[2]);
    // Compute the auxiliary fault plane
    //ierr = compearth_auxiliaryPlane(1, &kappa, &theta, &sigma,
    //                                &k2, &d2, &s2);
    // Just define some convention where smaller strike goes first
    //printf("%e %f %f %f\n", M0, kappa, theta, sigma);
    //printf("%f %f %f\n", k2, d2, s2);
    //------------------------------------------------------------------------//
    //                         do the standard decomposition                  //
    //------------------------------------------------------------------------//
    //double pAxis[3], bAxis[3], tAxis[3], fp1[3], fp2[3], isoPct, dcPct, devPct, clvdPct, Mw;
    double *pAxis, *bAxis, *tAxis, *fp1, *fp2, *isoPct, *dcPct, *devPct, *clvdPct, *Mw, *M0;
    M0 = (double *) calloc((size_t) nmt, sizeof(double));
    Mw = (double *) calloc((size_t) nmt, sizeof(double));
    fp1 = (double *) calloc((size_t) (3*nmt), sizeof(double));
    fp2 = (double *) calloc((size_t) (3*nmt), sizeof(double));
    pAxis = (double *) calloc((size_t) (3*nmt), sizeof(double));
    bAxis = (double *) calloc((size_t) (3*nmt), sizeof(double));
    tAxis = (double *) calloc((size_t) (3*nmt), sizeof(double));
    isoPct = (double *) calloc((size_t) nmt, sizeof(double));
    devPct = (double *) calloc((size_t) nmt, sizeof(double));
    dcPct = (double *) calloc((size_t) nmt, sizeof(double));
    clvdPct = (double *) calloc((size_t) nmt, sizeof(double));
    ierr = compearth_standardDecomposition(nmt, mtUSE, CE_USE,
                                           M0, Mw, fp1, fp2,
                                           pAxis, bAxis, tAxis,
                                           isoPct, devPct, dcPct, clvdPct);
    if (ierr != 0)
    {
        fprintf(stderr, "Failed to compute standard decomposition\n");
        return EXIT_FAILURE;
    }
    for (i=0; i<nmt; i++)
    {
        //--------------------------------------------------------------------//
        //                       Do the cMoPaD decomposition                  //
        //--------------------------------------------------------------------//
        // Copy moment tensor
        M[0][0] = mtUSE[6*i+0];           //Mrr
        M[1][1] = mtUSE[6*i+1];           //Mtt
        M[2][2] = mtUSE[6*i+2];           //Mpp
        M[0][1] = M[1][0] = mtUSE[6*i+3]; //Mrt
        M[0][2] = M[2][0] = mtUSE[6*i+4]; //Mrp
        M[1][2] = M[2][1] = mtUSE[6*i+5]; //Mtp
        // Mopad works in North, East, Down
        cin = USE;
        cloc = NED;
        cmopad_basis_transformMatrixM33(M, cin, cloc); //USE -> NED
        // Compute the isotropic, CLVD, DC decomposition
        ierr = cmopad_standardDecomposition(M, &src);
        if (ierr != 0)
        {
            printf("%s: Error in decomposition!\n", fcnm);
            return EXIT_FAILURE;
        }
        // Compute the princple axes and strikes, dips, and rakes
        ierr = cmopad_MT2PrincipalAxisSystem(iverb, &src);
        if (ierr != 0)
        {
            printf("%s: Error computing principal axis\n",fcnm);
            return EXIT_FAILURE;
        }
        // Compute the pressure, null, and, tension principal axes as
        // len,az,plunge
        cin = NED; //MoPaD is in north, east, down
        ierr = cmopad_Eigenvector2PrincipalAxis(cin, src.eig_pnt[0],
                                                src.p_axis,    pax);
        if (ierr != 0)
        {
            printf("%s: Error converting pax\n",fcnm);
            return EXIT_FAILURE;
        }
        ierr = cmopad_Eigenvector2PrincipalAxis(cin, src.eig_pnt[1],
                                                src.null_axis, bax);
        if (ierr != 0)
        {
            printf("%s: Error converting bax\n",fcnm);
            return EXIT_FAILURE;
        }
        ierr = cmopad_Eigenvector2PrincipalAxis(cin, src.eig_pnt[2],
                                                src.t_axis,    tax);
        if (ierr != 0)
        {
            printf("%s: Error converting tax\n",fcnm);
            return EXIT_FAILURE;
        }
        // Check it
        //printf("%d %d %e\n", i, i%9, Mw[i]);
        if (fabs(M0[i] - src.seismic_moment)/src.seismic_moment > 1.e-10)
        {
            fprintf(stderr, "failed M0 %f %f %e %d\n", M0[i], src.seismic_moment,
                    fabs(M0[i] - src.seismic_moment)/src.seismic_moment, i);
            return EXIT_FAILURE;
        }
        if (fabs(Mw[i] - src.moment_magnitude) > 1.e-10)
        {
            fprintf(stderr, "failed Mw %f %f %d\n", Mw[i], src.moment_magnitude, i);
            return EXIT_FAILURE;
        }
        if (fabs(isoPct[i] - src.ISO_percentage) > 1.e-10)
        {
            fprintf(stderr, "Failed iso pct %f %f\n",
                    isoPct[i], src.ISO_percentage);
            return EXIT_FAILURE;
        }
        if (fabs(dcPct[i] - src.DC_percentage) > 1.e-10)
        {
            fprintf(stderr, "Failed DC pct %f %f %d\n",
                    dcPct[i], src.DC_percentage, i);
            return EXIT_FAILURE;
        }
        if (fabs(devPct[i] - src.DEV_percentage) > 1.e-10)
        {
            fprintf(stderr, "Failed dev pct %f %f\n",
                    devPct[i], src.DEV_percentage);
            return EXIT_FAILURE;
        }
        if (fabs(clvdPct[i] - src.CLVD_percentage) > 1.e-10)
        {
            fprintf(stderr, "Failed clvd pct %f %f\n",
                    devPct[i], src.CLVD_percentage);
            return EXIT_FAILURE;
        }
        // Swap fault planes
        if (fabs(fp1[3*i+0] - src.fp1[0]) > 1.e-10)
        {
            if (fabs(fp1[3*i+0] - src.fp2[0]) > 1.e-10)
            {
                fprintf(stderr, "Failed strike 1 %f %f\n", fp1[3*i+0], src.fp2[0]);
                return EXIT_FAILURE;
            }
            if (fabs(fp1[3*i+1] - src.fp2[1]) > 1.e-10)
            {
                fprintf(stderr, "Failed dip 1 %f %f\n", fp1[3*i+1], src.fp2[1]);
                return EXIT_FAILURE;
            }
            if (fabs(fp1[3*i+2] - src.fp2[2]) > 1.e-10)
            {
               fprintf(stderr, "Failed rake 1 %f %f\n", fp1[3*i+2], src.fp2[2]);
               return EXIT_FAILURE;
            }
            if (fabs(fp2[3*i+0] - src.fp1[0]) > 1.e-10)
            {
                fprintf(stderr, "Failed strike 2 %f %f\n", fp2[3*i+0], src.fp1[0]);
                return EXIT_FAILURE;
            }
            if (fabs(fp2[3*i+1] - src.fp1[1]) > 1.e-10)
            {
                fprintf(stderr, "Failed dip 2 %f %f\n", fp2[3*i+1], src.fp1[1]);
                return EXIT_FAILURE;
            }
            if (fabs(fp2[3*i+2] - src.fp1[2]) > 1.e-10)
            {
                fprintf(stderr, "Failed rake 2 %f %f\n", fp2[3*i+2], src.fp1[2]);
                return EXIT_FAILURE;
            }
        }
        else
        {
            if (fabs(fp1[3*i+0] - src.fp1[0]) > 1.e-10)
            {
                fprintf(stderr, "Failed strike 1 %f %f\n",
                        fp1[3*i+0], src.fp1[0]);
                return EXIT_FAILURE;
            }
            if (fabs(fp1[3*i+1] - src.fp1[1]) > 1.e-10)
            {
                fprintf(stderr, "Failed dip 1 %f %f\n",
                        fp1[3*i+1], src.fp1[1]);
                return EXIT_FAILURE;
            }
            if (fabs(fp1[3*i+2] - src.fp1[2]) > 1.e-10)
            {
                fprintf(stderr, "Failed rake 1 %f %f\n",
                        fp1[3*i+2], src.fp1[2]);
                return EXIT_FAILURE;
            }
            if (fabs(fp2[3*i+0] - src.fp2[0]) > 1.e-10)
            {
                fprintf(stderr, "Failed strike 2 %f %f\n",
                       fp2[3*i+0], src.fp2[0]);
                return EXIT_FAILURE;
            }
            if (fabs(fp2[3*i+1] - src.fp2[1]) > 1.e-10)
            {
                fprintf(stderr, "Failed dip 2 %f %f\n",
                        fp2[3*i+1], src.fp2[1]);
                return EXIT_FAILURE;
            }
            if (fabs(fp2[3*i+2] - src.fp2[2]) > 1.e-10)
            {
                fprintf(stderr, "Failed rake 2 %f %f\n",
                        fp2[3*i+2], src.fp2[2]);
                return EXIT_FAILURE;
            }
        }
        if (fabs(pAxis[3*i+0] - pax[0]) > 1.e-10 ||
            fabs(pAxis[3*i+1] - pax[1]) > 1.e-10 ||
            fabs((pAxis[3*i+2] - pax[2])/pax[2]) > 1.e-10)
        {
            fprintf(stderr, "failed pressure axix: %f %f %f %f %f %f\n",
                    pAxis[0], pAxis[1], pAxis[2], pax[0], pax[1], pax[2]);
            return EXIT_FAILURE;
        }
        if (fabs(bAxis[3*i+0] - bax[0]) > 1.e-10 ||
            fabs(bAxis[3*i+1] - bax[1]) > 1.e-10 ||
            fabs((bAxis[3*i+2] - bax[2])/bax[2]) > 1.e-10)
        {
            fprintf(stderr, "Failed null axis: %f %f %f %f %f %f\n",
                    bAxis[0], bAxis[1], bAxis[2], bax[0], bax[1], bax[2]);
            return EXIT_FAILURE;
        }
        if (fabs(tAxis[3*i+0] - tax[0]) > 1.e-10 ||
            fabs(tAxis[3*i+1] - tax[1]) > 1.e-10 ||
            fabs((tAxis[3*i+2] - tax[2])/tax[2]) > 1.e-10)
        {
            fprintf(stderr, "Failed tension axis %f %f %f %f %f %f\n",
                    tAxis[0], tAxis[1], tAxis[2], tax[0], tax[1], tax[2]);
            return EXIT_FAILURE;
        }
        if (nmt == 1)
        {
            printf("Summary:\n");
            printf("Mw: %f\n", Mw[i]);
            printf("(strike,dip,rake): (%f,%f,%f)\n",
                   fp1[3*i+0], fp1[3*i+1], fp1[3*i+2]);
            printf("(strike,dip,rake): (%f,%f,%f)\n",
                   fp2[3*i+0], fp2[3*i+1], fp2[3*i+2]);
            printf("iso pct: %f\n", isoPct[i]);
            printf("dc pct: %f\n", dcPct[i]);
            printf("dev pct: %f\n", devPct[i]);
            printf("clvd pct: %f\n", clvdPct[i]);
            printf("\n");
        }
    }
    free(M0);
    free(Mw);
    free(fp1);
    free(fp2);
    free(pAxis);
    free(bAxis);
    free(tAxis);
    free(isoPct);
    free(devPct);
    free(dcPct);
    free(clvdPct);
    return EXIT_SUCCESS;
}

int main()
{
    struct mt_struct mt;
    const int nmt = 21*CE_CHUNKSIZE - 1;
    double *mtTest = (double *) calloc((size_t) (6*nmt), sizeof(double));
             //mtTest[2*6*CE_CHUNKSIZE];
    double xscal;
    int ierr, jmt;
    const int n6 = 6;
    const int incx = 1;
    //-----------------------------------1------------------------------------//
    mt.exp = 23.0 - 7.0;
    mt.mag = 5.1;
    mt.mt[0] =-4.360;
    mt.mt[1] = 3.200;
    mt.mt[2] = 1.170;
    mt.mt[3] =-0.794;
    mt.mt[4] = 1.650;
    mt.mt[5] = 2.060;
    mt.sdr1[0] = 77.0;
    mt.sdr1[1] = 47.0;
    mt.sdr1[2] =-62.0;
    mt.sdr2[0] = 219.0;
    mt.sdr2[1] = 50.0;
    mt.sdr2[2] =-117.0;
    mt.tax[0] = 4.49;
    mt.tax[1] = 1.0;
    mt.tax[2] = 328.0;
    mt.bax[0] = 0.56;
    mt.bax[1] = 20.0;
    mt.bax[2] = 237.0;
    mt.pax[0] =-5.04;
    mt.pax[1] = 70.0;
    mt.pax[2] = 61.0;
    xscal = pow(10.0, mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[0], 1);
    // Perform decomposition
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decomposiing 1\n");
        return EXIT_FAILURE;
    }
    //--------------------------------------2----------------------------------//
    mt.exp = 23.0 - 7.0;
    mt.mag = 5.0;
    mt.mt[0] =-4.480;
    mt.mt[1] = 1.440;
    mt.mt[2] = 3.040;
    mt.mt[3] = 0.603;
    mt.mt[4] = 0.722;
    mt.mt[5] =-1.870;
    mt.sdr1[0] =316.0;
    mt.sdr1[1] = 44.0;
    mt.sdr1[2] =-105.0;
    mt.sdr2[0] = 157.0;
    mt.sdr2[1] = 48.0;
    mt.sdr2[2] =-76.0;
    mt.tax[0] = 4.28;
    mt.tax[1] = 2.0;
    mt.tax[2] = 237.0;
    mt.bax[0] = 0.37;
    mt.bax[1] = 10.0;
    mt.bax[2] = 327.0;
    mt.pax[0] =-4.66;
    mt.pax[1] = 79.0;
    mt.pax[2] =137.0;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[6], 1);
    // Perform decomposition
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decomposiing 1\n");
        return EXIT_FAILURE;
    }
    //------------------------------------3-----------------------------------//
    mt.exp = 23.0 - 7.0;
    mt.mag = 4.9;
    mt.mt[0] =-2.460;
    mt.mt[1] = 0.207;
    mt.mt[2] = 2.250;
    mt.mt[3] = 0.793;
    mt.mt[4] = 0.267;
    mt.mt[5] =-0.363;
    mt.sdr1[0] =335.0;
    mt.sdr1[1] = 46.0;
    mt.sdr1[2] =-113.0;
    mt.sdr2[0] = 186.0;
    mt.sdr2[1] = 49.0;
    mt.sdr2[2] =-68.0;
    mt.tax[0] = 2.32;
    mt.tax[1] = 2.0;
    mt.tax[2] = 261.0;
    mt.bax[0] = 0.38;
    mt.bax[1] = 16.0;
    mt.bax[2] = 351.0;
    mt.pax[0] =-2.70;
    mt.pax[1] = 74.0;
    mt.pax[2] =165.0;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[12], 1);
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decomposing 3\n");
        return EXIT_FAILURE;
    }
    //-----------------------------------4------------------------------------//
    mt.exp = 23.0 - 7.0;
    mt.mag = 4.9;
    mt.mt[0] =-2.270;
    mt.mt[1] = 0.058;
    mt.mt[2] = 2.210;
    mt.mt[3] = 0.079;
    mt.mt[4] =-0.737;
    mt.mt[5] = 0.246;
    mt.sdr1[0] =190.0;
    mt.sdr1[1] = 36.0;
    mt.sdr1[2] =-84.0;
    mt.sdr2[0] = 3.0;
    mt.sdr2[1] = 54.0;
    mt.sdr2[2] =-94.0;
    mt.tax[0] = 2.35;
    mt.tax[1] = 9.0;
    mt.tax[2] = 96.0;
    mt.bax[0] = 0.04;
    mt.bax[1] = 4.0;
    mt.bax[2] = 5.0;
    mt.pax[0] =-2.39;
    mt.pax[1] = 80.0;
    mt.pax[2] =253.0;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[18], 1);
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error dcomposing 4\n");
        return EXIT_FAILURE;
    }
    //-----------------------------------5------------------------------------//
    mt.exp = 26.0 - 7.0;
    mt.mag = 6.5;
    mt.mt[0] = 0.745;
    mt.mt[1] =-0.036;
    mt.mt[2] =-0.709;
    mt.mt[3] =-0.242;
    mt.mt[4] =-0.048;
    mt.mt[5] = 0.208;
    mt.sdr1[0] =181.0;
    mt.sdr1[1] = 47.0;
    mt.sdr1[2] =114.0;
    mt.sdr2[0] = 328.0;
    mt.sdr2[1] = 48.0;
    mt.sdr2[2] = 67.0;
    mt.tax[0] = 0.82;
    mt.tax[1] = 73.0;
    mt.tax[2] = 166.0;
    mt.bax[0] =-0.05;
    mt.bax[1] = 17.0;
    mt.bax[2] = 344.0;
    mt.pax[0] =-0.77;
    mt.pax[1] = 1.0;
    mt.pax[2] = 74.0;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[24], 1);
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decomposing 5\n");
        return EXIT_FAILURE;
    }
    //-----------------------------------6------------------------------------//
    mt.exp = 24.0 - 7.0;
    mt.mag = 5.3;
    mt.mt[0] = 0.700;
    mt.mt[1] =-0.883;
    mt.mt[2] = 0.183;
    mt.mt[3] = 0.260;
    mt.mt[4] = 0.289;
    mt.mt[5] = 0.712;
    mt.sdr1[0] =329.0;
    mt.sdr1[1] = 54.0;
    mt.sdr1[2] =141.0;
    mt.sdr2[0] = 85.0;
    mt.sdr2[1] = 59.0;
    mt.sdr2[2] = 43.0;
    mt.tax[0] = 1.01;
    mt.tax[1] = 51.0;
    mt.tax[2] = 300.0;
    mt.bax[0] = 0.24;
    mt.bax[1] = 39.0;
    mt.bax[2] = 113.0;
    mt.pax[0] =-1.25;
    mt.pax[1] = 3.0;
    mt.pax[2] = 206.;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[30], 1);
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decomposing 6\n");
        return EXIT_FAILURE;
    }
    //-----------------------------------7------------------------------------//
    mt.exp = 23.0 - 7.0;
    mt.mag = 5.0;
    mt.mt[0] = 3.150;
    mt.mt[1] =-2.470;
    mt.mt[2] =-0.676;
    mt.mt[3] = 1.650;
    mt.mt[4] = 1.880;
    mt.mt[5] =-1.470;
    mt.sdr1[0] =252.0;
    mt.sdr1[1] = 28.0;
    mt.sdr1[2] =111.0;
    mt.sdr2[0] = 49.0;
    mt.sdr2[1] = 64.0;
    mt.sdr2[2] = 79.0;
    mt.tax[0] = 4.08;
    mt.tax[1] = 69.0;
    mt.tax[2] = 297.0;
    mt.bax[0] = 0.01;
    mt.bax[1] = 10.0;
    mt.bax[2] = 54.0;
    mt.pax[0] =-4.08;
    mt.pax[1] = 18.0;
    mt.pax[2] = 147.0;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[36], 1);
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decomposing 7\n");
        return EXIT_FAILURE;
    }
    //-----------------------------------8------------------------------------//
    mt.exp = 23.0 - 7.0;
    mt.mag = 4.8;
    mt.mt[0] =-1.870;
    mt.mt[1] = 0.488;
    mt.mt[2] = 1.380;
    mt.mt[3] =-0.057;
    mt.mt[4] = 0.600;
    mt.mt[5] = 0.664;
    mt.sdr1[0] = 36.0;
    mt.sdr1[1] = 38.0;
    mt.sdr1[2] =-76.0;
    mt.sdr2[0] = 199.0;
    mt.sdr2[1] = 53.0;
    mt.sdr2[2] =-101.0;
    mt.tax[0] = 1.80;
    mt.tax[1] = 8.0;
    mt.tax[2] = 296.0;
    mt.bax[0] = 0.18;
    mt.bax[1] = 9.0;
    mt.bax[2] = 205.0;
    mt.pax[0] =-1.99;
    mt.pax[1] = 78.0;
    mt.pax[2] = 69.0;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[42], 1);
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decomposing 8\n");
        return EXIT_FAILURE;
    }
    //-----------------------------------9------------------------------------//
    mt.exp = 23.0 - 7.0;
    mt.mag = 5.0;
    mt.mt[0] =-3.540;
    mt.mt[1] = 1.040;
    mt.mt[2] = 2.510;
    mt.mt[3] =-0.474;
    mt.mt[4] = 1.640;
    mt.mt[5] = 1.530;
    mt.sdr1[0] = 47.0;
    mt.sdr1[1] = 38.0;
    mt.sdr1[2] =-63.0;
    mt.sdr2[0] = 195.0;
    mt.sdr2[1] = 56.0;
    mt.sdr2[2] =-109.0;
    mt.tax[0] = 3.66;
    mt.tax[1] = 10.0;
    mt.tax[2] = 299.0;
    mt.bax[0] = 0.45;
    mt.bax[1] = 16.0;
    mt.bax[2] =206.0;
    mt.pax[0] =-4.10;
    mt.pax[1] = 71.0;
    mt.pax[2] = 58.0;
    xscal = pow(10.0,mt.exp);
    cblas_dscal(n6, xscal, mt.mt, incx);
    cblas_dcopy(n6, mt.mt, 1, &mtTest[48], 1);
    // now copy this
    for (jmt=9; jmt<nmt; jmt++)
    {
        cblas_dcopy(n6, &mtTest[6*((jmt-9)%9)], 1, &mtTest[6*jmt], 1);
    }
    ierr = decompose(1, mt.mt);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error decompsoing 9\n");
        return EXIT_FAILURE;
    }
    //-----------------------------Test 'em all-------------------------------//
    ierr = decompose(nmt, mtTest);
    if (ierr != EXIT_SUCCESS)
    {
        fprintf(stderr, "Error in bulk decomposition\n");
        return EXIT_FAILURE;
    }
    fprintf(stdout, "Success!\n");
    free(mtTest);
    return EXIT_SUCCESS;
}


/*
 * Author:  Nicu Tofan
 * License: BSD
 *
 * See below for getRealTime() license.
 */

#include <cblas.h>

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

#define test_m          2
#define test_k          3
#define test_n          4
#define test_m_extra    5
#define test_k_extra    7
#define test_n_extra    8

#define test_row_major  1
#define test_col_major  0

double getRealTime();

//! prints a matrix in either row major or column major order
static void printMatrix (double *data, int rows, int cols, int row_major)
{
    int i,j;

    // column header
    printf ("        ");
    for(j=0; j<cols;j++) {
        printf (" Col %2d  ", j+1);
    }
    printf ("\n");

    for(i=0; i<rows;i++) {
        printf ("Row %2d: ", i+1);
        for(j=0; j<cols;j++) {
            if (row_major) {
                printf ("%7.4f  ", data[i*cols+j]);
            } else {
                printf ("%7.4f  ", data[j*rows+i]);
            }
        }
        printf ("\n");
    }
    printf ("\n--------------------------------------------------------\n\n");
}

//! test printMatrix() to make sure it works
static void testPrintMatrix()
{
    printf ("==========================================================\n");
    printf ("Test print function\n");
    printf ("==========================================================\n\n");

    printf ("Matrix in row major order\n\n");
    double A[2*3] = {
        1, 2, 3,
        4, 5, 6
    };
    printMatrix (A, 2, 3, test_row_major);

    printf ("Matrix in column major order\n\n");
    double B[2*3] = {
        1, 4,
        2, 5,
        3, 6
    };
    printMatrix (B, 2, 3, test_col_major);
}

//! testing the extra-space
static void testMulColMajorExtraSpace()
{
    printf ("==========================================================\n");
    printf ("Extra space in matrix not used in ops\n");
    printf ("==========================================================\n\n");

    /*          MATRIX A[2x3]

                Col  1  Col  2  Col  3
        Row  1: 1.0000  2.0000  3.0000
        Row  2: 4.0000  5.0000  6.0000

    */

    printf ("A\n\n");
    double A[] = {
        1, 4, 9, 9, 9, // 9's will not affect the computation
        2, 5, 9, 9, 9,
        3, 6, 9, 9, 9
    };
    printMatrix (A, test_m_extra, test_k, test_col_major);

    /*          MATRIX B[3x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1: 1.0000  2.0000  3.0000  4.0000
        Row  2: 0.1000  0.2000  0.3000  0.4000
        Row  3: 0.0100  0.0200  0.0300  0.0400

    */
    printf ("B\n\n");
    double B[] = {
        1.0, 0.1, 0.01, 9.0, 9.0, 9.0, 9.0, // 9's will not affect the computation
        2.0, 0.2, 0.02, 9.0, 9.0, 9.0, 9.0,
        3.0, 0.3, 0.03, 9.0, 9.0, 9.0, 9.0,
        4.0, 0.4, 0.04, 9.0, 9.0, 9.0, 9.0,
    };
    printMatrix (B, test_k_extra, test_n, test_col_major);

    /*          MATRIX C[2x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1:  1.2300  2.4600  3.6900  4.9200
        Row  2:  4.5600  9.1200 13.6800 18.2400
    */
    double C[test_n_extra*test_n] = {0};

    printf ("C\n\n");
    // C = alpha*A*B+beta*C
    cblas_dgemm(
          CblasColMajor,
          CblasNoTrans, // TRANSA:
          CblasNoTrans, // TRANSB:
          test_m,   // A matrix: m rows, k columns
          test_n,   // B matrix: k rows, n columns
          test_k,   // C matrix: m rows, n columns
          1.0,      // alpha = 1.0
          A,        // A source matrix
          test_m_extra,   // true lenght of a column in A
                    //   if TRANSA = 'N' then >= m
                    //   if TRANSA = 'T' then >= k
          B,        // B source matrix
          test_k_extra,   // true lenght of a column in B
                    //   if TRANSB = 'N' then >= k
                    //   if TRANSB = 'T' then >= n
          0.0,      // beta = 0.0
          C,        // C destination matrix
          test_n_extra);  // true lenght of a row in C
                    //   must be >= m
    printMatrix (C, test_n_extra, test_n, test_col_major);
}

//! blas expects column major order
static void testMulColMajor()
{
    printf ("==========================================================\n");
    printf ("Test BLAS multiply - column major order\n");
    printf ("==========================================================\n\n");

    /*          MATRIX A[2x3]

                Col  1  Col  2  Col  3
        Row  1: 1.0000  2.0000  3.0000
        Row  2: 4.0000  5.0000  6.0000

    */

    printf ("A\n\n");
    double A[test_m*test_k] = {
        1, 4,
        2, 5,
        3, 6
    };
    printMatrix (A, test_m, test_k, test_col_major);

    /*          MATRIX B[3x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1: 1.0000  2.0000  3.0000  4.0000
        Row  2: 0.1000  0.2000  0.3000  0.4000
        Row  3: 0.0100  0.0200  0.0300  0.0400

    */
    printf ("B\n\n");
    double B[test_k*test_n] = {
        1.0, 0.1, 0.01,
        2.0, 0.2, 0.02,
        3.0, 0.3, 0.03,
        4.0, 0.4, 0.04
    };
    printMatrix (B, test_k, test_n, test_col_major);

    /*          MATRIX C[2x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1:  1.2300  2.4600  3.6900  4.9200
        Row  2:  4.5600  9.1200 13.6800 18.2400
    */
    double C[test_m*test_n] = {0};

    printf ("C\n\n");
    // http://www.math.utah.edu/software/lapack/lapack-blas/dgemm.html
    /*
    cblas_dgemm(
          'N',      // TRANSA: N - do not transpose A, T - transpose A
          'N',      // TRANSB: N - do not transpose A, T - transpose A
          test_m,   // A matrix: m rows, k columns
          test_n,   // B matrix: k rows, n columns
          test_k,   // C matrix: m rows, n columns
          1.0,      // alpha = 1.0
          A,        // A source matrix
          test_m,   // true lenght of a column in A
                    //   if TRANSA = 'N' then >= m
                    //   if TRANSA = 'T' then >= k
          B,        // B source matrix
          test_k,   // true lenght of a column in B
                    //   if TRANSB = 'N' then >= k
                    //   if TRANSB = 'T' then >= n
          0.0,      // beta = 0.0
          C,        // C destination matrix
          test_m);  // true lenght of a row in C
                    //   must be >= m
                    */
    // C = alpha*A*B+beta*C
    cblas_dgemm(
          CblasColMajor,
          CblasNoTrans, // TRANSA:
          CblasNoTrans, // TRANSB:
          test_m,   // A matrix: m rows, k columns
          test_n,   // B matrix: k rows, n columns
          test_k,   // C matrix: m rows, n columns
          1.0,      // alpha = 1.0
          A,        // A source matrix
          test_m,   // true lenght of a column in A
                    //   if TRANSA = 'N' then >= m
                    //   if TRANSA = 'T' then >= k
          B,        // B source matrix
          test_k,   // true lenght of a column in B
                    //   if TRANSB = 'N' then >= k
                    //   if TRANSB = 'T' then >= n
          0.0,      // beta = 0.0
          C,        // C destination matrix
          test_m);  // true lenght of a row in C
                    //   must be >= m
    printMatrix (C, test_m, test_n, test_col_major);
}

//! attempt the trickery
static void testMulRowMajor_trickery()
{
    printf ("==========================================================\n");
    printf ("Test BLAS multiply - row major order with trickery\n");
    printf ("==========================================================\n\n");

    /*          MATRIX A[2x3]

                Col  1  Col  2  Col  3
        Row  1: 1.0000  2.0000  3.0000
        Row  2: 4.0000  5.0000  6.0000

    */

    printf ("A\n\n");
    double A[test_m*test_k] = {
        1, 2, 3,
        4, 5, 6
    };
    printMatrix (A, test_m, test_k, test_row_major);

    /*          MATRIX B[3x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1: 1.0000  2.0000  3.0000  4.0000
        Row  2: 0.1000  0.2000  0.3000  0.4000
        Row  3: 0.0100  0.0200  0.0300  0.0400

    */
    printf ("B\n\n");
    double B[test_k*test_n] = {
        1.00, 2.00, 3.00, 4.00,
        0.10, 0.20, 0.30, 0.40,
        0.01, 0.02, 0.03, 0.04
    };
    printMatrix (B, test_k, test_n, test_row_major);

    /*          MATRIX C[2x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1:  1.2300  2.4600  3.6900  4.9200
        Row  2:  4.5600  9.1200 13.6800 18.2400
    */
    double C[test_m*test_n] = {0};

    printf ("C\n\n");

    // C = alpha*A*B+beta*C
    cblas_dgemm(
        CblasColMajor,
        CblasNoTrans, // TRANSA:
        CblasNoTrans, // TRANSB:
        test_n,   // B matrix: k rows, n columns
        test_m,   // A matrix: m rows, k columns
        test_k,   // C matrix: m rows, n columns
        1.0,      // alpha = 1.0
        B,        // B source matrix
        test_n,   // true lenght of a column in B
                  //   if TRANSB = 'N' then >= k
                  //   if TRANSB = 'T' then >= n
        A,        // A source matrix
        test_k,   // true lenght of a column in A
                  //   if TRANSA = 'N' then >= m
                  //   if TRANSA = 'T' then >= k
        0.0,      // beta = 0.0
        C,        // C destination matrix
        test_n);  // true lenght of a row in C
                  //   must be >= m
    printMatrix (C, test_m, test_n, test_row_major);
}

//! build in version
static void testMulRowMajor()
{
    printf ("==========================================================\n");
    printf ("Test BLAS multiply - row major order without trickery\n");
    printf ("==========================================================\n\n");

    /*          MATRIX A[2x3]

                Col  1  Col  2  Col  3
        Row  1: 1.0000  2.0000  3.0000
        Row  2: 4.0000  5.0000  6.0000

    */

    printf ("A\n\n");
    double A[test_m*test_k] = {
        1, 2, 3,
        4, 5, 6
    };
    printMatrix (A, test_m, test_k, test_row_major);

    /*          MATRIX B[3x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1: 1.0000  2.0000  3.0000  4.0000
        Row  2: 0.1000  0.2000  0.3000  0.4000
        Row  3: 0.0100  0.0200  0.0300  0.0400

    */
    printf ("B\n\n");
    double B[test_k*test_n] = {
        1.00, 2.00, 3.00, 4.00,
        0.10, 0.20, 0.30, 0.40,
        0.01, 0.02, 0.03, 0.04
    };
    printMatrix (B, test_k, test_n, test_row_major);

    /*          MATRIX C[2x4]

                Col  1  Col  2  Col  3  Col  4
        Row  1:  1.2300  2.4600  3.6900  4.9200
        Row  2:  4.5600  9.1200 13.6800 18.2400
    */
    double C[test_m*test_n] = {0};

    printf ("C\n\n");

    // C = alpha*A*B+beta*C
    cblas_dgemm(
          CblasRowMajor,
          CblasNoTrans, // TRANSA:
          CblasNoTrans, // TRANSB:
          test_m,   // A matrix: m rows, k columns
          test_n,   // B matrix: k rows, n columns
          test_k,   // C matrix: m rows, n columns
          1.0,      // alpha = 1.0
          A,        // A source matrix
          test_k,   // true lenght of a column in A
                    //   if TRANSA = 'N' then >= m
                    //   if TRANSA = 'T' then >= k
          B,        // B source matrix
          test_n,   // true lenght of a column in B
                    //   if TRANSB = 'N' then >= k
                    //   if TRANSB = 'T' then >= n
          0.0,      // beta = 0.0
          C,        // C destination matrix
          test_n);  // true lenght of a row in C
                    //   must be >= m
    printMatrix (C, test_m, test_n, test_col_major);
}

#undef test_m
#undef test_n
#undef test_k

#define test_m          500
#define test_k          600
#define test_n          700

#define test_spins      5

// ---------------------------------------------------------------------
#define IMPLEMENTATION_ROW_MAJOR \
    cblas_dgemm( \
                CblasRowMajor, CblasNoTrans, CblasNoTrans, test_m, test_n, \
                test_k, 1.0, A, test_k, B, test_n, 0.0, C, test_n); \
    startTime = getRealTime (); \
    for(i=0; i<test_spins;i++) { \
        cblas_dgemm( \
                    CblasRowMajor, CblasNoTrans, CblasNoTrans, test_m, test_n, \
                    test_k, 1.0, A, test_k, B, test_n, 0.0, C, test_n); \
    } \
    endTime = getRealTime (); \
    printf ("- Build-in implementation - row major (%d) = %lf\n", test_spins, (endTime - startTime) );
// ---------------------------------------------------------------------
#define IMPLEMENTATION_TRICKERY \
    cblas_dgemm( \
                CblasColMajor, CblasNoTrans, CblasNoTrans, test_n, test_m, \
                test_k, 1.0, B, test_n, A, test_k, 0.0, C, test_n); \
    startTime = getRealTime (); \
    for(i=0; i<test_spins;i++) { \
        cblas_dgemm( \
                    CblasColMajor, CblasNoTrans, CblasNoTrans, test_n, test_m, \
                    test_k, 1.0, B, test_n, A, test_k, 0.0, C, test_n); \
    } \
    endTime = getRealTime (); \
    printf ("- Trickery implementation (%d)= %lf\n", test_spins, (endTime - startTime) );
// ---------------------------------------------------------------------
#define IMPLEMENTATION_COL_MAJOR \
    cblas_dgemm( \
                CblasColMajor, CblasNoTrans, CblasNoTrans, test_m, test_n, \
                test_k, 1.0, A, test_m, B, test_k, 0.0, C, test_m); \
    startTime = getRealTime (); \
    for(i=0; i<test_spins;i++) { \
        cblas_dgemm( \
                    CblasColMajor, CblasNoTrans, CblasNoTrans, test_m, test_n, \
                    test_k, 1.0, A, test_m, B, test_k, 0.0, C, test_m); \
    } \
    endTime = getRealTime (); \
    printf ("- Build-in implementation - column major (%d) = %lf\n", test_spins, (endTime - startTime) );
// ---------------------------------------------------------------------

//! test my undestanding of the parameters
void compareBuildInWithTrickery()
{

    printf ("==========================================================\n");
    printf ("Compare these two methods\n");
    printf ("==========================================================\n\n");

    // srand((unsigned)time(NULL));

    double startTime, endTime;
    int i;

    double *A = (double*)malloc(test_m*test_k*sizeof(double));
    int a_tot = test_m*test_k;
    double *iter = A;
    for (i = 0; i<a_tot; i++) {
        *iter = ((double)rand()/(double)RAND_MAX);;
        iter++;
    }

    double *B = (double*)malloc(test_k*test_n*sizeof(double));
    int b_tot = test_k*test_n;
    iter = B;
    for (i = 0; i<b_tot; i++) {
        *iter = ((double)rand()/(double)RAND_MAX);;
        iter++;
    }

    double *C = (double*)malloc(test_m*test_n*sizeof(double));
    iter = C;
    int c_tot = test_m*test_n;
    for (i = 0; i<c_tot; i++) {
        *iter = 0.0;
        iter++;
    }

    IMPLEMENTATION_ROW_MAJOR;
    IMPLEMENTATION_TRICKERY;
    IMPLEMENTATION_COL_MAJOR;

    IMPLEMENTATION_TRICKERY;
    IMPLEMENTATION_ROW_MAJOR;
    IMPLEMENTATION_COL_MAJOR;

    IMPLEMENTATION_ROW_MAJOR;
    IMPLEMENTATION_COL_MAJOR;
    IMPLEMENTATION_TRICKERY;

    IMPLEMENTATION_COL_MAJOR;
    IMPLEMENTATION_ROW_MAJOR;
    IMPLEMENTATION_TRICKERY;

    IMPLEMENTATION_COL_MAJOR;
    IMPLEMENTATION_TRICKERY;
    IMPLEMENTATION_ROW_MAJOR;

    IMPLEMENTATION_TRICKERY;
    IMPLEMENTATION_COL_MAJOR;
    IMPLEMENTATION_ROW_MAJOR;

}

int main(int argc, char **argv)
{
    testPrintMatrix();
    testMulColMajorExtraSpace();
    testMulColMajor();
    testMulRowMajor_trickery();
    testMulRowMajor();

    compareBuildInWithTrickery();

    return 0;
}

/*
 * Author:  David Robert Nadeau
 * Site:    http://NadeauSoftware.com/
 * License: Creative Commons Attribution 3.0 Unported License
 *          http://creativecommons.org/licenses/by/3.0/deed.en_US
 *
 * http://nadeausoftware.com/articles/2012/04/c_c_tip_how_measure_elapsed_real_time_benchmarking
 */
#if defined(_WIN32)
#include <Windows.h>

#elif defined(__unix__) || defined(__unix) || defined(unix) || (defined(__APPLE__) && defined(__MACH__))
#include <unistd.h>	/* POSIX flags */
#include <time.h>	/* clock_gettime(), time() */
#include <sys/time.h>	/* gethrtime(), gettimeofday() */

#if defined(__MACH__) && defined(__APPLE__)
#include <mach/mach.h>
#include <mach/mach_time.h>
#endif

#else
#error "Unable to define getRealTime( ) for an unknown OS."
#endif

/**
 * Returns the real time, in seconds, or -1.0 if an error occurred.
 *
 * Time is measured since an arbitrary and OS-dependent start time.
 * The returned real time is only useful for computing an elapsed time
 * between two calls to this function.
 */
double getRealTime( )
{
#if defined(_WIN32)
    FILETIME tm;
    ULONGLONG t;
#if defined(NTDDI_WIN8) && NTDDI_VERSION >= NTDDI_WIN8
    /* Windows 8, Windows Server 2012 and later. ---------------- */
    GetSystemTimePreciseAsFileTime( &tm );
#else
    /* Windows 2000 and later. ---------------------------------- */
    GetSystemTimeAsFileTime( &tm );
#endif
    t = ((ULONGLONG)tm.dwHighDateTime << 32) | (ULONGLONG)tm.dwLowDateTime;
    return (double)t / 10000000.0;

#elif (defined(__hpux) || defined(hpux)) || ((defined(__sun__) || defined(__sun) || defined(sun)) && (defined(__SVR4) || defined(__svr4__)))
    /* HP-UX, Solaris. ------------------------------------------ */
    return (double)gethrtime( ) / 1000000000.0;

#elif defined(__MACH__) && defined(__APPLE__)
    /* OSX. ----------------------------------------------------- */
    static double timeConvert = 0.0;
    if ( timeConvert == 0.0 )
    {
        mach_timebase_info_data_t timeBase;
        (void)mach_timebase_info( &timeBase );
        timeConvert = (double)timeBase.numer /
            (double)timeBase.denom /
            1000000000.0;
    }
    return (double)mach_absolute_time( ) * timeConvert;

#elif defined(_POSIX_VERSION)
    /* POSIX. --------------------------------------------------- */
#if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0)
    {
        struct timespec ts;
#if defined(CLOCK_MONOTONIC_PRECISE)
        /* BSD. --------------------------------------------- */
        const clockid_t id = CLOCK_MONOTONIC_PRECISE;
#elif defined(CLOCK_MONOTONIC_RAW)
        /* Linux. ------------------------------------------- */
        const clockid_t id = CLOCK_MONOTONIC_RAW;
#elif defined(CLOCK_HIGHRES)
        /* Solaris. ----------------------------------------- */
        const clockid_t id = CLOCK_HIGHRES;
#elif defined(CLOCK_MONOTONIC)
        /* AIX, BSD, Linux, POSIX, Solaris. ----------------- */
        const clockid_t id = CLOCK_MONOTONIC;
#elif defined(CLOCK_REALTIME)
        /* AIX, BSD, HP-UX, Linux, POSIX. ------------------- */
        const clockid_t id = CLOCK_REALTIME;
#else
        const clockid_t id = (clockid_t)-1;	/* Unknown. */
#endif /* CLOCK_* */
        if ( id != (clockid_t)-1 && clock_gettime( id, &ts ) != -1 )
            return (double)ts.tv_sec +
                (double)ts.tv_nsec / 1000000000.0;
        /* Fall thru. */
    }
#endif /* _POSIX_TIMERS */

    /* AIX, BSD, Cygwin, HP-UX, Linux, OSX, POSIX, Solaris. ----- */
    struct timeval tm;
    gettimeofday( &tm, NULL );
    return (double)tm.tv_sec + (double)tm.tv_usec / 1000000.0;
#else
    return -1.0;		/* Failed. */
#endif
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <unistd.h>

#ifdef GSL_FOUND
#include <gsl/gsl_integration.h>
#endif

#include "core_allvars.h"
#include "core_init.h"
#include "core_mymalloc.h"
#include "core_cool_func.h"

/* These functions do not need to be exposed externally */
double integrand_time_to_present(const double a, void *param);
void set_units(struct params *run_params);
void read_snap_list(const int ThisTask, struct params *run_params);
double time_to_present(const double z, struct params *run_params);

#ifdef HDF5
#include "io/io_save_hdf5.h"
#endif

void init(const int ThisTask, struct params *run_params)
{
    run_params->Age = mymalloc(ABSOLUTEMAXSNAPS*sizeof(run_params->Age[0]));

    set_units(run_params);

    read_snap_list(ThisTask, run_params);

    //Hack to fix deltaT for snapshot 0
    //This way, galsnapnum = -1 will not segfault.
    run_params->Age[0] = time_to_present(1000.0, run_params);//lookback time from z=1000
    run_params->Age++;

    for(int i = 0; i < run_params->Snaplistlen; i++) {
        run_params->ZZ[i] = 1 / run_params->AA[i] - 1;
        run_params->Age[i] = time_to_present(run_params->ZZ[i], run_params);
    }

    run_params->a0 = 1.0 / (1.0 + run_params->Reionization_z0);
    run_params->ar = 1.0 / (1.0 + run_params->Reionization_zr);

    read_cooling_functions();
    if(ThisTask == 0) {
        printf("cooling functions read\n\n");
    }

#if 0
#ifdef HDF5
    if(HDF5Output) {
        calc_hdf5_props();
    }
#endif
#endif

}

void set_units(struct params *run_params)
{

    run_params->UnitTime_in_s = run_params->UnitLength_in_cm / run_params->UnitVelocity_in_cm_per_s;
    run_params->UnitTime_in_Megayears = run_params->UnitTime_in_s / SEC_PER_MEGAYEAR;
    run_params->G = GRAVITY / pow(run_params->UnitLength_in_cm, 3) * run_params->UnitMass_in_g * pow(run_params->UnitTime_in_s, 2);
    run_params->UnitDensity_in_cgs = run_params->UnitMass_in_g / pow(run_params->UnitLength_in_cm, 3);
    run_params->UnitPressure_in_cgs = run_params->UnitMass_in_g / run_params->UnitLength_in_cm / pow(run_params->UnitTime_in_s, 2);
    run_params->UnitCoolingRate_in_cgs = run_params->UnitPressure_in_cgs / run_params->UnitTime_in_s;
    run_params->UnitEnergy_in_cgs = run_params->UnitMass_in_g * pow(run_params->UnitLength_in_cm, 2) / pow(run_params->UnitTime_in_s, 2);

    run_params->EnergySNcode = run_params->EnergySN / run_params->UnitEnergy_in_cgs * run_params->Hubble_h;
    run_params->EtaSNcode = run_params->EtaSN * (run_params->UnitMass_in_g / SOLAR_MASS) / run_params->Hubble_h;

    // convert some physical input parameters to internal units
    run_params->Hubble = HUBBLE * run_params->UnitTime_in_s;

    // compute a few quantitites
    run_params->RhoCrit = 3.0 * run_params->Hubble * run_params->Hubble / (8 * M_PI * run_params->G);
}

void read_snap_list(const int ThisTask, struct params *run_params)
{
    char fname[MAX_STRING_LEN+1];

    snprintf(fname, MAX_STRING_LEN, "%s", run_params->FileWithSnapList);
    FILE *fd = fopen(fname, "r");
    if(fd == NULL) {
        printf("can't read output list in file '%s'\n", fname);
        ABORT(0);
    }

    run_params->Snaplistlen = 0;
    do {
        if(fscanf(fd, " %lg ", &(run_params->AA[run_params->Snaplistlen])) == 1) {
            run_params->Snaplistlen++;
        } else {
            break;
        }
    } while(run_params->Snaplistlen < run_params->MAXSNAPS);
    fclose(fd);

    if(ThisTask == 0) {
        printf("found %d defined times in snaplist\n", run_params->Snaplistlen);
    }
}

double time_to_present(const double z, struct params *run_params)
{
    const double end_limit = 1.0;
    const double start_limit = 1.0/(1 + z);
    double result=0.0;
#ifdef GSL_FOUND
#define WORKSIZE 1000
    gsl_function F;
    gsl_integration_workspace *workspace;
    double abserr;

    workspace = gsl_integration_workspace_alloc(WORKSIZE);
    F.function = &integrand_time_to_present;
    F.params = run_params;

    gsl_integration_qag(&F, start_limit, end_limit, 1.0 / run_params->Hubble,
                        1.0e-9, WORKSIZE, GSL_INTEG_GAUSS21, workspace, &result, &abserr);

    gsl_integration_workspace_free(workspace);

#undef WORKSIZE
#else
    /* Do not have GSL - let's integrate numerically ourselves */
    const double step  = 1e-7;
    const int64_t nsteps = (end_limit - start_limit)/step;
    result = 0.0;
    const double y0 = integrand_time_to_present(start_limit + 0*step, run_params);
    const double yn = integrand_time_to_present(start_limit + nsteps*step, run_params);
    for(int64_t i=1; i<nsteps; i++) {
        result  += integrand_time_to_present(start_limit + i*step, run_params);
    }

    result = (step*0.5)*(y0 + yn + 2.0*result);
#endif

    /* convert into Myrs/h (I think -> MS 23/6/2018) */
    const double time = 1.0 / run_params->Hubble * result;

    // return time to present as a function of redshift
    return time;
}

double integrand_time_to_present(const double a, void *param)
{
    const struct params *run_params = (struct params *) param;
    return 1.0 / sqrt(run_params->Omega / a + (1.0 - run_params->Omega - run_params->OmegaLambda) + run_params->OmegaLambda * a * a);
}


/*
 * BRAINS
 * (B)LR (R)everberation-mapping (A)nalysis (I)n AGNs with (N)ested (S)ampling
 * Yan-Rong Li, liyanrong@ihep.ac.cn
 * Thu, Aug 4, 2016
 */

/*!
 *  \file smooth.c
 *  \brief smoothing functions using FFT provided by GSL.
 *
 *  References: Press et al., Numerical Recipes, Chapter 13.1.
 *              GSL documents
 */

///////////////THIS FILE IS DEPRECATED///////////////////////////

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <mpi.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_fft_halfcomplex.h>

#include "brains.h"

int nd_fft;

double *resp, *data_fft;
double *resp_cmp, *data_fft_cmp, *data_fft_inverse;

gsl_fft_real_wavetable * real_data, *real_resp;
gsl_fft_complex_wavetable * cmp_data;
gsl_fft_real_workspace * work_data, *work_resp;
gsl_fft_complex_workspace * work_cmp;

/*!
 * This function initiates workspace for FFT.
 */
void smooth_init(int nv, const double *transv)
{
  //nd_fft = parset.n_vel_recon>=n_vel_data? parset.n_vel_recon : n_vel_data;
  nd_fft = nv;
  //printf("%d\n", nd_fft);
  real_data = gsl_fft_real_wavetable_alloc (nd_fft);
  real_resp = gsl_fft_real_wavetable_alloc (nd_fft);
  cmp_data = gsl_fft_complex_wavetable_alloc (nd_fft);

  work_data = gsl_fft_real_workspace_alloc (nd_fft);
  work_resp = gsl_fft_real_workspace_alloc (nd_fft);
  work_cmp = gsl_fft_complex_workspace_alloc (nd_fft);

  resp = malloc(nd_fft * sizeof(double));
  data_fft = malloc(nd_fft * sizeof(double));

  resp_cmp = malloc(nd_fft * 2 * sizeof(double));
  data_fft_cmp = malloc(nd_fft *2 * sizeof(double));
  data_fft_inverse = malloc(nd_fft *2 * sizeof(double));

  // initialize response and its fft.
  int i;
  double sigV, dV, tot;

  sigV = parset.InstRes / VelUnit;
  dV = transv[1] - transv[0];

  /* setup response, whose negective-time part is wrapped around and stored at the right hand*/
  tot = 0.0;
  for (i = 0; i<nd_fft/2; i++)
  {
    resp[i] = 1.0/sqrt(2.0*M_PI)/sigV * exp(-0.5*(i*dV)*(i*dV)/sigV/sigV);
    tot += resp[i];
  }
  for (i = nd_fft-1; i>= nd_fft/2; i--)
  {
    resp[i] = 1.0/sqrt(2.0*M_PI)/sigV * exp(-0.5*((i-nd_fft)*dV)*((i-nd_fft)*dV)/sigV/sigV);
    tot += resp[i];
  }

  /* normalize response */
  for(i=0; i<nd_fft; i++)
  {
    //resp[i] /= (tot * dV);
    resp[i] /= (tot);
  }

  /* FFT of response */
  gsl_fft_real_transform(resp, 1, nd_fft, real_resp, work_resp);
  gsl_fft_halfcomplex_unpack(resp, resp_cmp, 1, nd_fft);
}

/*!
 * This function finalizes FFT.
 */
void smooth_end()
{

  gsl_fft_real_wavetable_free(real_data);
  gsl_fft_real_wavetable_free(real_resp);
  gsl_fft_complex_wavetable_free(cmp_data);

  gsl_fft_real_workspace_free(work_data);
  gsl_fft_real_workspace_free(work_resp);
  gsl_fft_complex_workspace_free(work_cmp);

  free(resp);
  free(data_fft);

  free(resp_cmp);
  free(data_fft_cmp);
  free(data_fft_inverse);

}

/*!
 * This function performs FFT-smoothing to 2d line.
 */
void line_gaussian_smooth_2D_FFT(const double *transv, double *fl2d, int nl, int nv)
{
  int i, j;
  double dV;

  dV = transv[1] - transv[0];

  for(j=0; j<nl; j++)
  {
    memcpy(data_fft, &fl2d[j*nv], nv*sizeof(double));

    /* FFT of line */
    gsl_fft_real_transform(data_fft, 1, nd_fft, real_data, work_data);
    gsl_fft_halfcomplex_unpack(data_fft, data_fft_cmp, 1, nd_fft);

    /* complex multiply and inverse FFT */
    for(i=0; i<nd_fft; i++)
    {
      data_fft_inverse[i*2] = data_fft_cmp[i*2]*resp_cmp[i*2] - data_fft_cmp[i*2+1]*resp_cmp[i*2+1];
      data_fft_inverse[i*2+1] = data_fft_cmp[i*2]*resp_cmp[i*2+1] + data_fft_cmp[i*2+1]*resp_cmp[i*2];
    }
    gsl_fft_complex_inverse(data_fft_inverse, 1, nd_fft, cmp_data, work_cmp);

    for(i=0; i<nv; i++)
    {
      /* take into account the velocity grid width */
      //fl2d[j*nv + i] = data_fft_inverse[i*2] * dV;
      fl2d[j*nv + i] = data_fft_inverse[i*2];
    }
  }
  return;
}

/*!
 * test smoothing function, assumed line profile.
 */
void line_gaussian_smooth_2D_FFT_test(const double *transv, double *fl2d, int nl, int nv)
{
  int i, j;
  double sigV, dV, tot;

  sigV = parset.InstRes / VelUnit;
  dV = transv[1] - transv[0];

  tot = 0.0;
  for (i = 0; i<nd_fft/2; i++)
  {
    resp[i] = 1.0/sqrt(2.0*M_PI)/sigV * exp(-0.5*(i*dV)*(i*dV)/sigV/sigV);
    tot += resp[i];
  }
  for (i = nd_fft-1; i>= nd_fft/2; i--)
  {
    resp[i] = 1.0/sqrt(2.0*M_PI)/sigV * exp(-0.5*((i-nd_fft)*dV)*((i-nd_fft)*dV)/sigV/sigV);
    tot += resp[i];
  }

  for(i=0; i<nd_fft; i++)
  {
    resp[i] /= (tot * dV);
  }
  gsl_fft_real_transform(resp, 1, nd_fft, real_resp, work_resp);
  gsl_fft_halfcomplex_unpack(resp, resp_cmp, 1, nd_fft);

  for(j=0; j<nl; j++)
  {
    //memcpy(data_fft, &fl2d[j*nv], nv*sizeof(double));
    for(i=0; i<nv; i++)
    {
      data_fft[i] =   exp( - 0.5 * pow(transv[i]/(1500.0/VelUnit), 2.0));
    }

    gsl_fft_real_transform(data_fft, 1, nd_fft, real_data, work_data);
    gsl_fft_halfcomplex_unpack(data_fft, data_fft_cmp, 1, nd_fft);

    for(i=0; i<nd_fft; i++)
    {
      data_fft_inverse[i*2] = data_fft_cmp[i*2]*resp_cmp[i*2] - data_fft_cmp[i*2+1]*resp_cmp[i*2+1];
      data_fft_inverse[i*2+1] = data_fft_cmp[i*2]*resp_cmp[i*2+1] + data_fft_cmp[i*2+1]*resp_cmp[i*2];
    }
    gsl_fft_complex_inverse(data_fft_inverse, 1, nd_fft, cmp_data, work_cmp);

    for(i=0; i<nv; i++)
    {
      // take into account the velocity grid width
      fl2d[j*nv + i] = data_fft_inverse[i*2] * dV;
    }
  }
  return;
}

/*!
 * convolution test.
 */
void smooth_test()
{
  int i, n = 205;
  double data[n], resp[n];
  double data_cmp[n*2], resp_cmp[n*2];
  double ans[n*2];

  gsl_fft_real_wavetable * real, *real_resp;
  gsl_fft_complex_wavetable * hc;
  gsl_fft_real_workspace * work, *work_resp;
  gsl_fft_complex_workspace * work_cmp;

  for (i = 0; i < n; i++)
  {
    data[i] = 1.0/sqrt(2.0*M_PI)/6.0 * exp(-0.5*(i-50.0)*(i-50.0)/6.0/6.0);
  }

  for (i = 0; i < n/2; i++)
  {
    resp[i] = 1.0/sqrt(2.0*M_PI)/10.0 * exp(-0.5*(i-0.0)*(i-0.0)/10.0/10.0);
  }
  for (i = n-1; i >= n/2; i--)
  {
    resp[i] = 1.0/sqrt(2.0*M_PI)/10.0 * exp(-0.5*(i-n)*(i-n)/10.0/10.0);
  }

  for (i = 0; i < n; i++)
  {
    printf ("%d: %e\n", i, data[i]);
  }
  printf ("\n");

  work = gsl_fft_real_workspace_alloc (n);
  real = gsl_fft_real_wavetable_alloc (n);

  work_resp = gsl_fft_real_workspace_alloc (n);
  real_resp = gsl_fft_real_wavetable_alloc (n);

  work_cmp = gsl_fft_complex_workspace_alloc(n);
  hc = gsl_fft_complex_wavetable_alloc (n);

  gsl_fft_real_transform (data, 1, n, real, work);
  gsl_fft_real_transform (resp, 1, n, real_resp, work_resp);

  gsl_fft_halfcomplex_unpack(data, data_cmp, 1, n);
  gsl_fft_halfcomplex_unpack(resp, resp_cmp, 1, n);

  for(i=0; i<n; i++)
  {
    ans[i*2] = data_cmp[i*2]*resp_cmp[i*2] - data_cmp[i*2+1]*resp_cmp[i*2+1];
    ans[i*2+1] = data_cmp[i*2]*resp_cmp[i*2+1] + data_cmp[i*2+1]*resp_cmp[i*2];
  }

  gsl_fft_complex_inverse(ans, 1, n, hc, work_cmp);

  for (i = 0; i < n; i++)
  {
    printf ("%d: %e %e\n", i,  ans[i*2], ans[i*2+1]);
  }

  gsl_fft_real_wavetable_free (real);
  gsl_fft_real_wavetable_free (real_resp);
  gsl_fft_real_workspace_free (work);
  gsl_fft_real_workspace_free (work_resp);
  gsl_fft_complex_wavetable_free (hc);
  gsl_fft_complex_workspace_free(work_cmp);
  return;
}


#include <iostream>
#include <vector>
#include <stdio.h>
#include <gsl\gsl_randist.h>
#include <gsl\gsl_rng.h>
#include <time.h>
#include <SFML\Graphics.hpp>
#ifndef _HEART_H
#define _HEART_H

class heart
{
	private:
		int L;  //sytem size length for now square

		std::vector<std::vector<int>> state;  //value indicates whether in excited state refactory or resting
		std::vector<std::vector<int>> new_state;  //using new_state and state to distinguish newly excited cells from previously excited
		std::vector<std::vector<int>> functional; //1 if functional 0 if disfunctional
		std::vector<std::vector<int>> bond; //1 if bond to site above current cell, 0 if no bond above site
		std::vector<std::vector<int>> unablated; // Defaults to zero initial value
		int heart_time; //internal clock
		//model parameters
		int tau; //refactory period
		int T; // pacemaker rate
		double nu, epsilom, delta; //probability vertical connection exists, probability of misfire given faulty and probability cell faulty

		//random number generator
		const gsl_rng_type*Type;
		gsl_rng*r;

		int total_bonds;
		int total_functional;
		int total_excited;

	public:

		heart()
		{

		}
		heart(const int _L,  int _T, int _tau ,double _nu, double _epsilom, double _delta)
		{
			L=_L;
			nu= _nu;
			tau=_tau;
			T=_T;
			epsilom=_epsilom;
			delta=_delta;
			heart_time = T;

			state.resize(L);
			new_state.resize(L);
			functional.resize(L);
			bond.resize(L);
			unablated.resize(L);
			//pacemaker action
			for(unsigned i=0; i<L; i++)
			{
				state[i].resize(L,0);  // all cells eligible for excitation
				new_state[i].resize(L,0);
				functional[i].resize(L,0);// none disfunctional at the moment
				bond[i].resize(L, 1); // all transverse bonds present at the moment
				unablated[i].resize(L,1);
			}
		//	total_functional = L*L;
		//	total_bonds = L*L;
			total_excited =0;//could be useful avoiding calling size of some vector

			 //HAVE NOT USED gsl_rng in class before need to make sure is working correctly

			//  environment variable GSL_RNG_TYPE
			gsl_rng_env_setup();

			Type = gsl_rng_mt19937;//random number generator using the Mersenne Twister algorithm
			r = gsl_rng_alloc (Type);

			gsl_rng_set(r,time(NULL));//random number generator seeded with time
			//gsl_rng_set(r,0);//random number generator seeded with time

			//assigning cells as having a bond above them or not and functional or not functional
			for(int i=0; i<L; i++)
			{
				for(int j=0; j<L; j++)
				{

					bond[i][j] = (gsl_rng_uniform(r)<nu);
					functional[i][j] = (gsl_rng_uniform(r)>delta);// inequality reversed
					//double x =  gsl_rng_uniform(r);
				//	std::cout <<x<< '\t' << (x<nu) << std::endl;

				}
			}

		}
		void operate()//one time step
		{
			total_excited =0;

			//Relax excited cells
				for(int i=0; i<L ;i++)
			{
				for(int j=0; j<L; j++)
				{
					if(state[i][j]==1)//searching 4 neighbours of excited cell trying to excite if resting and if don't misfire and if there is a bond
					{
						new_state[((i+1)<L)*(i+1)][j]+=(state[((i+1)<L)*(i+1)][j]<1)*bond[i][j]*(1-(1-functional[((i+1)<L)*(i+1)][j])*(gsl_rng_uniform(r)<epsilom))*unablated[((i+1)<L)*(i+1)][j];

						new_state[(i-1)*(i>0)+(L-1)*(i<1)][j]+=(state[(i-1)*(i>0)+(L-1)*(i<1)][j]<1)*bond[(i-1)*(i>0)+(L-1)*(i<1)][j]*(1-(1-functional[(i-1)*(i>0)+(L-1)*(i<1)][j])*(gsl_rng_uniform(r)<epsilom))*unablated[(i-1)*(i>0)+(L-1)*(i<1)][j];

						new_state[i][(j+1)*(j<L-1)] += (j<L-1)*(state[i][(j+1)*(j<L-1)]<1)*(1-(1-functional[i][(j+1)*(j<L-1)])*(gsl_rng_uniform(r)<epsilom))*unablated[i][(j+1)*(j<L-1)];

						new_state[i][(j-1)*(j>0)]+= (j>0)*(state[i][(j-1)*(j>0)]<1)*(1-(1-functional[i][(j-1)*(j>0)])*(gsl_rng_uniform(r)<epsilom))*unablated[i][(j-1)*(j>0)];
					}
				}
			}
				//end relaxing

			//evolving cells that were not excited previously or now (some subtlty to those possibly excited now)
			for(int i=0;i<L;i++)
			{
				for(int j=0; j<L; j++)
				{
					new_state[i][j] += (state[i][j]>0);
					//if state was excited or refactory before +1 states that were resting may have been newly excited or not but this ignores them
					new_state[i][j] *= (state[i][j]<tau);//promote to resting after end of refactory
					total_excited += (new_state[i][j]<2)*new_state[i][j];
				}
			}

				//SAN
			for(int i=0; i<L; i++)
			{
				new_state[i][0] += (state[i][0]<1)*(heart_time>T-1)*(1-(1-functional[i][0])*(gsl_rng_uniform(r)<epsilom));
					//will do something every T steps should also add in misfire
			}

			heart_time++;
			heart_time *= (heart_time<T+1);
			//end SAN

			state=new_state;

		}
		int get_state(int i, int j)//perhaps have alternative for 2d array
		{

			return state[i][j];

		}
		void set_state(int i, int j, int val)
		{

			state[i][j]=val;
		}
		int get_function(int i, int j)
		{
			return functional[i][j];
		}
		void set_function(int i, int j, int val)
		{
			functional[i][j]=val;
		}
		int get_bond(int i, int j)
		{
			return bond[i][j];
		}
		void set_bond(int i, int j, int val)
		{
			bond[i][j]=val;
		}
		int get_tau()
		{
			return tau;
		}
		int get_T()
		{
			return T;
		}
		void age_nu()
		{

		}
		void set_disfunction()
		{

		}
		int get_excited()
		{
			return total_excited;
		}
		double colour(int i, int j)
		{
			return (1-state[i][j]/(1.0*tau))*(state[i][j]>1);
			//return state[i][j]/(1.0*T);

		}
		void interact(sf::RenderWindow & renderWindow, int length)
		{
			sf::Event event;
			float x;
			float y;

			float Mx = sf::Mouse::getPosition(renderWindow).x;
			float My = sf::Mouse::getPosition(renderWindow).y;
			//std::cout << Mx << " " << My << std::endl;
			while (renderWindow.pollEvent(event))
			{
			switch(event.type)
			{
			case sf::Event::KeyPressed:

				state[My/length][Mx/length] = 0;
				new_state[My/length][Mx/length] = 0;
				unablated[My/length][Mx/length] = 0;

			}
			}
		}
};
#endif