/**
 *
 *	\file gpsins.c - Implements an integrated GPS-Inertial navigation system.
 *
 * 	This program implements an integrated GPS-Inertial navigation system.  Data is
 *	retrieved from shared memory and processed in two threads.  The high speed thread
 *	implements the inertial navigation mechanisation.  The low speed thread combines
 *	the inertial navigation solution with GPS measurements in a tightly coupled
 *	architecture.  The output is stored back into shared memory.
 *
 * 	This program implements a 17 state tightly coupled EKF.  Note
 * 	that the filter states are organised such that the first 15 states
 *	are identical to those used by a 15 state loosely coupled EKF.
 *
 *	The filter uses an error state formulation.  The EKF_x_hat vector contains the state
 *	error rather than the full state.  The full state is stored in regular arrays of
 *	Pos_LLH, Vel_NED and C_BN representing the Latitude, Longitude, Height, North, East
 *	and Down Velocity, and attitude direction cosine matrix (DCM).
 *
 *      \code
 *	Filter States
 *	=============
 *	x1  - Latitude Error (radians)
 * 	x2  - Longitude Error (radians)
 * 	x3  - Height Error (metres)
 * 	x4  - North Velocity Error (m/s)
 * 	x5  - East Velocity Error (m/s)
 *	x6  - Down Velocity Error (m/s)
 * 	x7  - North Tilt Error (radians)
 *	x8  - East Tilt Error (radians)
 *	x9  - Down Tilt Error (radians)
 * 	x10 - X Accelerometer Bias Error (m/s/s)
 *	x11 - Y Accelerometer Bias Error (m/s/s)
 * 	x12 - Z Accelerometer Bias Error (m/s/s)
 * 	x13 - X Gyroscope Bias Error (rad/s)
 * 	x14 - Y Gyroscope Bias Error (rad/s)
 * 	x15 - Z Gyroscope Bias Error (rad/s)
 * 	x16 - Receiver Clock Bias Error (metres)
 * 	x17 - Receiver Clock Frequency Error (metres/sec)
 *
 *	Measurement States - assuming 'n' satellite measurements available
 * 	==================
 * 	z1 - zn - GPS Pseudorange Measurements
 * 	zn+1 - 2zn - GPS Pseudorange Rate (doppler) measurements
 *
 *	These states are not yet implemented...
 *	2nz+1 - Barometric Altitude Error (metres)
 *	2zn+2 - Compass Heading Error (radians)
 *	\endcode
 *
 *	\author Duncan Greer
 *	\version "$Id: gpsins.c 955 2007-12-06 06:38:04Z greerd $"
 */

/* this is an additonal comment */

/* if we are using rtai, declare it here */
/* #define USE_RTAI */
#undef USE_RTAI

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <strings.h>
#include <string.h>
#include <errno.h>
#include <pthread.h>
#include <stdlib.h>
#include <signal.h>

#ifndef _SVID_SOURCE
#define _SVID_SOURCE
#endif

#include <sys/shm.h>
#include <sys/ipc.h>

#include <mqueue.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <spawn.h>

#include <sys/mman.h>

#ifdef USE_RTAI
#include <rtai_lxrt.h>
#include <rtai_bits.h>
#else
#include <sys/ioctl.h>
#include <linux/rtc.h>
#endif

#include <math.h>
#include "gpsins.h"
#include "gps.h"
#include "../../novatelstore/src/shmdef.h"
#include "matrix.h"

#define MODULE_NAME 	"[GPSINS]"

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

#define CONTROL_MQUEUE_NAME 	"/control-queue"
#define GPSINS_MQUEUE_NAME 	"/gpsins-queue"
#define	INS_MQUEUE_NAME		"/ins-queue"

#define CMD_SHUTDOWN 1
#define CMD_RUNLOOP 2

#define MAX_DIR_LENGTH 1024

//int Shutdown = 0;

/** thread to control execution of the program */
void *ControlThread(void *ThreadArgs);

/** thread to perform INS solution - 100Hz */
void *INSThread(void *ThreadArgs);

/** thread to perform GPS-INS solution - 1 Hz */
void *GPSINSThread(void *ThreadArgs);

/* INS globals */
double C_BN[3][3];
double Pos_LLH[3];
double Vel_NED[3];
double Acc_NED[3];
double UserClock[2];
double GyroBias[3];
double AccBias[3];

int iShutdown = 0;

/* state variables */
gsl_matrix *EKF_P;
gsl_vector *EKF_x_hat;

/**
 *	\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;
}

/**
* main
*/
int main(int argc, char *argv[])
{
	/* generic return value */
	int iReturnValue;

#ifndef USE_RTAI
	int iRTCDevice;
	long lRTCBuffer;
#endif

	/*
	int iMessage = 0;
	char cMessage = 0;
	*/

	message mMessage;

	/* thread paramter/control objects */
	pthread_t ptControlThread;
	pthread_t ptINSThread;
	pthread_t ptGPSINSThread;

	/* pointers to GPS and IMU data respectively */
//	GPSData *ptGPSData;
//	IMUData *ptIMUData;

	/* shared memory objects */
//	int shmGPSId, shmIMUId;
//	key_t shmGPSKey, shmIMUKey;

	/* message queue */
	mqd_t	mqdControl;

	struct mq_attr mqdControlAttr;

	/** GPS INS Thread Message Queue (POSIX) */
	mqd_t	mqdGPSINS;

	/** INS Thread Message Queue (POSIX) */
	mqd_t	mqdINS;

	/** storage for gps store pid */
//	pid_t pidGPSStore;

	/* storage for current working directory */
//	char azcCurrentWorkingDirectory[MAX_DIR_LENGTH];
//	char azcLaunchPath[MAX_DIR_LENGTH+20];
//	char *ptr;

	/* storage for GPSStore Arguments */
//	char *azcGPSStoreArgs[4] = {"gpsstore","/dev/ttyUSB0","230400",NULL};

	sigset_t ssSigSet;

	log("Initialising Program");

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

	sigemptyset(&ssSigSet);
	sigaddset(&ssSigSet,SIGRTMIN);
	pthread_sigmask(SIG_BLOCK, &ssSigSet, NULL);

	mqdControlAttr.mq_maxmsg = 10;
	mqdControlAttr.mq_msgsize = sizeof(message);
	mqdControlAttr.mq_flags = 0;

	/*  initialise message	*/
	memset(&mMessage,0,sizeof(message));

	/*  create a control message queue */
	mqdControl = mq_open(CONTROL_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL|O_NONBLOCK, (mode_t)0666, &mqdControlAttr);
	if(mqdControl == -1)
	{
		perror("mq_open(): CONTROL_MQUEUE_NAME");
		exit(-1);
	}

	/* setup gps ins message queue */
#ifdef USE_RTAI
	mqdGPSINS = mq_open(GPSINS_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL|O_NONBLOCK, (mode_t)0666, &mqdControlAttr);
#else
	mqdGPSINS = mq_open(GPSINS_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL, (mode_t)0666, &mqdControlAttr);
#endif
	if(mqdGPSINS == -1)
	{
		perror("mq_open(): GPSINS_MQUEUE_NAME");
		exit(-1);
	}
	/* setup ins message queue */
	mqdINS = mq_open(INS_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL|O_NONBLOCK, (mode_t)0666, &mqdControlAttr);
	if(mqdINS == -1)
	{
		perror("mq_open(): INS_MQUEUE_NAME");
		exit(-1);
	}
	/*
	ptmqdControl = (mqd_t *)malloc(sizeof(mqd_t));
	*ptmqdControl = mqdControl;
	*/

	/* start control thread */
	iReturnValue = pthread_create(&ptControlThread,NULL,ControlThread,(void *)&mqdControl);
	if(iReturnValue < 0)
	{
		perror("pthread_create():");
	}

	/* debug message */
	/*
	fprintf(stdout,"%s GPS Data: %d bytes, IMU Data: %d bytes\n",MODULE_NAME, sizeof(GPSData), sizeof(IMUData));
	*/

//	shmGPSKey = GPSDATA_KEY;
//	shmIMUKey = IMUDATA_KEY;

	/*. create shared memory space for storing the data */
//	if((shmGPSId = shmget(shmGPSKey, sizeof(GPSData), IPC_CREAT | 0666)) < 0)
//	{
//      	perror("shmget(): allocating GPS data storage");
//   		exit(-1);
//	}

	/* attach */
//	if((ptGPSData = shmat(shmGPSId, NULL, 0)) == (GPSData *) -1)
//	{
//       	perror("shmat(): attaching GPS Data Storage");
//        	exit(-1);
 //   	}

//	if((shmIMUId = shmget(shmIMUKey,sizeof(GPSData), IPC_CREAT | 0666)) < 0)
//	{
//		perror("shmget(): allocating IMU data storage");
//		exit(-1);
//	}
//	if((ptIMUData = shmat(shmIMUId, NULL, 0)) == (IMUData *)-1)
//	{
//		perror("shmat(): attaching IMU data storage");
//		exit(-1);
//	}

	/* intialise shared memory */
//	memset(ptGPSData,0,sizeof(GPSData));
//	memset(ptIMUData,0,sizeof(IMUData));

	/* get current working directory - used for spawn call */
//	ptr = getcwd(azcCurrentWorkingDirectory,(size_t) MAX_DIR_LENGTH);
//	strcat(azcLaunchPath,azcCurrentWorkingDirectory);
//	strcat(azcLaunchPath,"/");
//	strcat(azcLaunchPath,azcGPSStoreArgs[0]);

	/* start GPS and INS data stores */
//	fprintf(stdout,"%s Launching GPSStore: %s\n",MODULE_NAME,azcLaunchPath);
//	iReturnValue = posix_spawn(&pidGPSStore,		/* storage for child pid */
//		    azcCurrentWorkingDirectory, /* current working directory */
//		    NULL,			/* posix_spawn_file_actions_t */
//		    NULL,			/* posix_spawnattr_t */
//		    azcGPSStoreArgs,		/* GPS Store Args - serial port, baud */
//		    NULL);			/* environment */

//	if(iReturnValue != 0)
//	{
//		perror("posix_spawn(): GPSStore");
//	}

	/* start GPS and INS threads  */
	iReturnValue = pthread_create(&ptINSThread,NULL,INSThread,(void *)&mqdINS);
	if(iReturnValue < 0)
	{
		perror("pthread_create(): INS Thread");
	}

	iReturnValue = pthread_create(&ptGPSINSThread,NULL,GPSINSThread,(void *)&mqdGPSINS);
	if(iReturnValue < 0)
	{
		perror("pthread_create(): GPS-INS Thread");
	}

#ifndef USE_RTAI
	/* open real time clock if we are not using RTAI */
	iRTCDevice = open("/dev/rtc",O_RDONLY);
	if (iRTCDevice < 0)
	{
		perror("open: RTC: ");
	} else
	{
		ioctl(iRTCDevice,RTC_UIE_ON,NULL);
		log("Successfully opened RTC");
	}
#endif
	while(iShutdown == 0)
	{
		iReturnValue = mq_receive(mqdControl,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0)
		{
			/*
				perror("mq_receive():");
			*/
		}
		/*
		log("Received Message");
		fprintf(stdout,"Rx: %d, %d\n",iReturnValue, mMessage.iMessageType);
		*/
		if(mMessage.iMessageType == 1)
		{
			// exit
			log("Received Shutdown Message");
			iShutdown = 1;
			break;
		}

		/* this read will return at 1 second intervals */
		read(iRTCDevice,&lRTCBuffer,sizeof(long));

		/* send message to run threads */
		mMessage.iMessageType = CMD_RUNLOOP;
		iReturnValue = mq_send(mqdGPSINS,(const char *)&mMessage,sizeof(message),1);

	}

	log("Shutting Down");

	/* send message to shutdown threads */
	mMessage.iMessageType = CMD_SHUTDOWN;
	iReturnValue = mq_send(mqdGPSINS,(const char *)&mMessage,sizeof(message),1);
	if(iReturnValue < 0)
	{
		perror("mq_send(): Thread Shutdown");
	}
	iReturnValue = mq_send(mqdINS,(const char *)&mMessage,sizeof(message),1);
	if(iReturnValue < 0)
	{
		perror("mq_send(): Thread Shutdown");
	}

	iReturnValue = mq_send(mqdControl,(const char *)&mMessage,sizeof(message),1);
	if(iReturnValue < 0)
	{
		perror("mq_send(): Thread Shutdown");
	}

	/* wait for threads to shutdown */
	//sleep(2);
	fprintf(stdout,"%s Waiting for INS thread to join...", MODULE_NAME);
	pthread_join(ptINSThread,NULL);
	fprintf(stdout,"DONE\n");

	fprintf(stdout,"%s Waiting for GPS-INS thread to join...", MODULE_NAME);
	pthread_join(ptGPSINSThread,NULL);
	fprintf(stdout,"DONE\n");

	fprintf(stdout,"%s Waiting for Control thread to join...", MODULE_NAME);
	pthread_join(ptControlThread,NULL);
	fprintf(stdout,"DONE\n");

#ifndef USE_RTAI
	/* close rtc */
	close(iRTCDevice);
#endif

	/* close and unlink message queues */
	fprintf(stdout,"%s Unlinking Message Queues...",MODULE_NAME);
	mq_close(mqdControl);
	mq_unlink(CONTROL_MQUEUE_NAME);
	mq_close(mqdGPSINS);
	mq_unlink(GPSINS_MQUEUE_NAME);
	mq_close(mqdINS);
	mq_unlink(INS_MQUEUE_NAME);

	fprintf(stdout,"DONE\n");

	log("Exiting");
	pthread_exit(NULL);
	return 0;
} /* end main */

/*
* ControlThread - controls execution of the program
*/
void *ControlThread(void *ThreadArgs)
{
	char c[100];
	mqd_t *ptmqdControl;
	int iReturnValue;

	message mMessage;

	fprintf(stdout,"%s Control Thread Initialised - press 'q <enter>' to exit!\n",MODULE_NAME);

	/* initialise message	 */
	memset(&mMessage,0,sizeof(message));

	/* read arguments */
	ptmqdControl = (mqd_t *)ThreadArgs;

	while(scanf("%s",c))
	{
		/* check for message (non blocking) */
		iReturnValue = mq_receive(*ptmqdControl,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0)
		{
			perror("mq_recieve(): Control Thread");
		}

		if(mMessage.iMessageType == CMD_SHUTDOWN)
		{
			log("Control Thread Received SHUTDOWN command");
			//pthread_exit(NULL);
			break;
		}

		switch(strlen(c))
		{
			case 1:
				/* command character */
				switch(c[0])
				{
					case 'q':
						mMessage.iMessageType = CMD_SHUTDOWN;

						/* send shutdown message */
						iReturnValue = mq_send(*ptmqdControl,
								(const char *)&mMessage,
								sizeof(message),
								1);
						if(iReturnValue < 0)
						{
							perror("mq_send():");
						}
						break;

					default:
						fprintf(stderr,"%s Unknown command: %c\n",MODULE_NAME, c[0]);
						break;
				}
			default:
				/* do nothing */
				break;
		}

	}

	fprintf(stdout,"%s Control Thread Exiting\n", MODULE_NAME);

	/* never reached */
	pthread_exit(NULL);
}

int iINSThreadCounter;

void *INSThread(void *ThreadArgs)
{
	/* message storage */
	message mMessage;

	/* generic return storage */
	int iReturnValue;

	/* message queue pointer */
	mqd_t *ptmqdINS;

	/* timer variables */
	int iSigNumber;
	sigset_t ssSigSet;
	struct sigevent seSigEvent;
	timer_t tPeriodicTimer;
	struct timespec first,period;
	struct itimerspec required,old;
	long lThreadPeriod = 10000000; /* 10ms = 100 hz */

	/* shared memory variables */
	int iUseSHM = 1;
	shm_struct *shm = NULL;
	int iSHM_FID;

	double EarthRate[3];
	double Tecef2ned[3][3];
	double RM,RP;
	double Lat,Long;
	double LatDot,LongDot;
	double sLat,cLat;
	double OMEGA_b[3][3];
	double Coriolis[3];
	double Acc_XYZ[3];
	double omega_x,omega_y,omega_z;
	double OMEGA_e_n[3];
	double OMEGA_in[3][3];

	fprintf(stdout,"%s INS Thread Started\n",MODULE_NAME);

	/* initialise message	*/
	memset(&mMessage,0,sizeof(message));

	/* setup message queue */
	ptmqdINS = (mqd_t *)ThreadArgs;

	/* setup thread timer */
	seSigEvent.sigev_notify = SIGEV_SIGNAL;
	seSigEvent.sigev_signo = SIGRTMIN;

	clock_gettime(CLOCK_REALTIME, &first);
	first.tv_sec = first.tv_sec + 1;

	fprintf(stdout,"INS: First loop runs at %ld.%ld\n", (long)first.tv_sec,(long)first.tv_nsec);

	period.tv_sec = 0;
	period.tv_nsec = lThreadPeriod;

	required.it_value = first;
	required.it_interval = period;

	iReturnValue = timer_create(CLOCK_REALTIME,&seSigEvent,&tPeriodicTimer);
	if(iReturnValue < 0)
	{
		perror("INS: timer_create():");
	}

	sigemptyset(&ssSigSet);
	sigaddset(&ssSigSet,SIGRTMIN);
	//pthread_sigmask(SIG_UNBLOCK, &ssSigSet, NULL);

	iReturnValue = timer_settime(tPeriodicTimer,TIMER_ABSTIME,&required, &old);
	if(iReturnValue < 0)
	{
		perror("INS: timer_settime():");
	}

       	/* 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);
		}
	  }
	}

	/* initialise INS parameters */
	C_BN[0][0] = 1.0;
	C_BN[1][1] = 1.0;
	C_BN[2][2] = 1.0;

	GyroBias[0] = 0.0;
	GyroBias[1] = 0.0;
	GyroBias[2] = 0.0;

	AccBias[0] = 0.0;
	AccBias[1] = 0.0;
	AccBias[2] = 0.0;

	/* position */
	Pos_LLH[0] = -27.5*DEG2RAD;
	Pos_LLH[1] = 153.0*DEG2RAD;
	Pos_LLH[2] = 90.0;

	/* velocity */
	Vel_NED[0] = 0.0;
	Vel_NED[1] = 0.0;
	Vel_NED[2] = 0.0;

	/* acceleration */
	Acc_NED[0] = 0.0;
	Acc_NED[1] = 0.0;
	Acc_NED[2] = -LOCAL_GRAVITY;

	Acc_XYZ[0] = 0.0;
	Acc_XYZ[1] = 0.0;
	Acc_XYZ[2] = -LOCAL_GRAVITY;

	EarthRate[0] = 0.0;
	EarthRate[1] = 0.0;
	EarthRate[2] = OMEGA_e;

	Coriolis[0] = 0.0;
	Coriolis[1] = 0.0;
	Coriolis[2] = 0.0;

	RM = WGS84_a;
	RP = WGS84_a;
	WGS84_calcRnRe(Pos_LLH[0],  &RM, &RP);

	OMEGA_e_n[0] = 0.0;
	OMEGA_e_n[1] = 0.0;
	OMEGA_e_n[2] = 0.0;

	log("INS Thread Commencing Periodic");

	while(1)
	{

		//fprintf(stdout,"Waiting...");
		/* wait for signal */
		sigwait(&ssSigSet, &iSigNumber);

		iINSThreadCounter++;

		/* check for messages */
		iReturnValue = mq_receive(*ptmqdINS,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0 && errno != EAGAIN)
		{
			perror("INS: mq_recieve(): ");
			//fprintf(stdout,"%d",iReturnValue);
		}

		if(mMessage.iMessageType == CMD_SHUTDOWN)
		{
			break;
		}

		/* get current lat and long */
		Lat = Pos_LLH[0];
		Long = Pos_LLH[1];
		cLat = cos(Lat);
		sLat = sin(Lat);

		T_ECEF2NED(Lat, Long, (double **)Tecef2ned);

		/* attitude update */
		omega_x = shm->CurrentIMUData.dRate[0] - GyroBias[0];
		omega_y = shm->CurrentIMUData.dRate[1]  - GyroBias[1];
		omega_z = shm->CurrentIMUData.dRate[2]  - GyroBias[2];

		/* form the skew symmetric form */
		OMEGA_b[0][0] = 0.0;
		OMEGA_b[0][1] = -omega_z;
		OMEGA_b[0][2] =  omega_y;
		OMEGA_b[1][0] =  omega_z;
		OMEGA_b[1][1] = 0.0;
		OMEGA_b[1][2] = -omega_x;
		OMEGA_b[2][0] = -omega_y;
		OMEGA_b[2][1] = omega_x;
		OMEGA_b[2][2] = 0.0;

		/* navigation frame rotation correction */
		OMEGA_in[0][0] = 0.0;
		OMEGA_in[0][1] = -OMEGA_e * sLat;
		OMEGA_in[0][2] = 0.0;
		OMEGA_in[1][0] = OMEGA_e * sLat;
		OMEGA_in[1][1] = 0.0;
		OMEGA_in[1][2] = -OMEGA_e * cLat;
		OMEGA_in[2][0] = 0.0;
		OMEGA_in[2][1] = OMEGA_e * cLat;
		OMEGA_in[2][2] = 0.0;

		/* DCM Update */
		/* C_BN = C_BN + (C_BN * OMEGA_b - OMEGA_in * C_BN)*INS_DT */

		/* acceleration calculation */
		/* Acc_NED = C_BN * Acc_XYZ */
		Acc_XYZ[0] = shm->CurrentIMUData.dAcc[0] - AccBias[0];
		Acc_XYZ[1] = shm->CurrentIMUData.dAcc[1] - AccBias[1];
		Acc_XYZ[2] = shm->CurrentIMUData.dAcc[2] - AccBias[2] -LOCAL_GRAVITY;

		MatMul331(C_BN, Acc_XYZ, Acc_NED);

		/* correct vertical channel for gravity */
		Acc_NED[2] = Acc_NED[2] + LOCAL_GRAVITY;

		/* coriolis correction */
		MatMul331(Tecef2ned, EarthRate, OMEGA_e_n);

		/* Coriolis = cross(2 * OMEGA_e_n,[0,0,0]); */

		Acc_NED[0] = Acc_NED[0] - Coriolis[0];
		Acc_NED[1] = Acc_NED[1] - Coriolis[1];
		Acc_NED[2] = Acc_NED[2] - Coriolis[2];

		/* velocity update */
		Vel_NED[0] = Vel_NED[0] + Acc_NED[0] * INS_DT;
		Vel_NED[1] = Vel_NED[1] + Acc_NED[1] * INS_DT;
		Vel_NED[2] = Vel_NED[2] + Acc_NED[2] * INS_DT;

		WGS84_calcRnRe(Pos_LLH[0],  &RM, &RP);
		LatDot = Vel_NED[0] / (RM + Pos_LLH[2]);
		LongDot = Vel_NED[1] / (cos(Pos_LLH[0])*(RP + Pos_LLH[2]));

		/* position update */
		Pos_LLH[0] = Pos_LLH[0] + LatDot * INS_DT;
		Pos_LLH[1] = Pos_LLH[1] + LongDot * INS_DT;
		Pos_LLH[2] = Pos_LLH[2] + -Vel_NED[2] * INS_DT;

		/* covariance update */
		/* P = PHI * P * PHI' + Qd */

	}
	/* exit */
	fprintf(stdout,"%s INS Thread Exiting\n",MODULE_NAME);
	pthread_exit(NULL);
}

void *GPSINSThread(void *ThreadArgs)
{
	int iRunThread = 1;

	/* pointer to message queue designator passed from arguments */
	mqd_t *ptmqdGPSINS;

	/* generic return value storage */
	int iReturnValue;

	/* generic message storage */
	message mMessage;

#ifdef USE_RTAI
	/* real time task */
	static RT_TASK *rttGPSINS;
#endif

	/* KF stuff */
	int iNumberStates,iNumberMeasurements;

	/* matrices */
	gsl_matrix *EKF_Q;
	gsl_matrix *EKF_PHI; /* phi ~= I + Fdt */
	gsl_matrix *EKF_F;
	gsl_matrix *EKF_P_minus;
	gsl_matrix *tempP;

	gsl_matrix *EKF_H;
	gsl_matrix *EKF_R;
	gsl_matrix *EKF_K;
	gsl_matrix *EKF_V;
	gsl_matrix *EKF_Vinv;
	gsl_matrix *tempV;
	gsl_matrix *tempK;

	gsl_matrix *KH;
	gsl_matrix *I_17by17;
	gsl_matrix *IminusKH;
	gsl_matrix *KHPminus;
	gsl_matrix *KRKT;
	gsl_matrix *tempKRKT;

	/* svd stuff */
	gsl_vector *SVD_S;
	gsl_vector *SVD_Sinv;
	gsl_matrix *SVD_SinvMat;
	gsl_vector *SVDWork;
	gsl_matrix *SVD_V;
	gsl_matrix *SVD_U;
	gsl_matrix *tempNN;

	/* vectors */
	gsl_vector *EKF_z;
	gsl_vector *EKF_deltaz;
	gsl_vector *EKF_x_hat_out;

	gsl_vector *temp1;

	double x_gyro_beta;
	double y_gyro_beta;
	double z_gyro_beta;
	double x_accel_beta;
	double y_accel_beta;
	double z_accel_beta;

	double user_x,user_y,user_z,user_t;
	double user_xdot,user_ydot,user_zdot,user_tdot;
	double sat_x,sat_y,sat_z,sat_t;
	double sat_xdot,sat_ydot,sat_zdot,sat_tdot;
	double ION_Alpha[4];
	double ION_Beta[4];
	unsigned short usPRN;
	double range,rangerate;
	double geo_vel_to_sat;
	double delta_pr_omegaedot;

	double Rm,Rp;
	double Rmh,Rph,Rg;

	double IonoDelay[NOVATEL_MAXCHANNELS],TropoDelay[NOVATEL_MAXCHANNELS];
	double SV_Azimuth[NOVATEL_MAXCHANNELS];
	double SV_Elevation[NOVATEL_MAXCHANNELS];
//	double llh[3];
//	double ecef[3];
	double dTransmissionTime;

	GPSTime gtObservationTime;
	RANGE	GPSRangeData;
	GPSEPHEM GPSEphemData[NOVATEL_MAXPRNS];
	int iIndex;
	int iNumberValidObservations;
	int iNumberValidEphemeris;
	int iUseSV[NOVATEL_MAXCHANNELS];

	double UserPos[3];
	double UserVel[3];

	double Tecef2ned[3][3];
	double Tned2ecef[3][3];

	double SVPos_calc[4];
	double SVVel_calc[4];
	double SVAcc_calc[4];

	double C_BN_new[3][3];

	double EKF_H_E[3][3];
	double EKF_H_LTP[3][3];

	double del_alpha,del_beta,del_gamma;
	double del_att_skew[3][3];

	/* matrix counter variables */
	int iRowIndex,iColIndex;

	/* shared memory variables */
	int iUseSHM = 1;
	shm_struct *shm = NULL;
	int iSHM_FID;

	/* log message */
	fprintf(stdout,"%s GPS-INS Thread Started\n",MODULE_NAME);

	/* initialise message	 */
	memset(&mMessage,0,sizeof(message));

	/* setup message queue */
	ptmqdGPSINS = (mqd_t *)ThreadArgs;

	/* setup EKF */
	iNumberStates = NUMBER_STATES;
	iNumberMeasurements = 0;

	EKF_x_hat = gsl_vector_alloc(iNumberStates);
	EKF_x_hat_out = gsl_vector_alloc(iNumberStates);

	EKF_Q = gsl_matrix_alloc(iNumberStates,iNumberStates);
	EKF_P = gsl_matrix_alloc(iNumberStates,iNumberStates);
	EKF_P_minus = gsl_matrix_alloc(iNumberStates,iNumberStates);
	tempP = gsl_matrix_alloc(iNumberStates,iNumberStates);

	EKF_PHI = gsl_matrix_alloc(iNumberStates,iNumberStates);
	EKF_F = gsl_matrix_alloc(iNumberStates,iNumberStates);

	I_17by17 = gsl_matrix_alloc(iNumberStates,iNumberStates);

	KH = gsl_matrix_alloc(iNumberStates,iNumberStates);
	IminusKH = gsl_matrix_alloc(iNumberStates,iNumberStates);
	KHPminus = gsl_matrix_alloc(iNumberStates,iNumberStates);

	gsl_matrix_set_identity(EKF_P);
	gsl_matrix_scale(EKF_P,10.0);

	temp1 = gsl_vector_alloc(3);

	x_gyro_beta = 1/100.0;
	y_gyro_beta = 1/100.0;
	z_gyro_beta = 1/100.0;

	x_accel_beta = 1/100.0;
	y_accel_beta = 1/100.0;
	z_accel_beta = 1/100.0;

	UserClock[0] = 0.0;
	UserClock[1] = 0.0;

#ifdef USE_RTAI
	/* make this thread real time */
	rt_set_oneshot_mode();
	start_rt_timer(0);
	rttGPSINS = rt_task_init_schmod(nam2num("GPSINS"),
				10,
				0,
				0,
				SCHED_FIFO,
				0);
 	if (rttGPSINS == NULL)
	{
		perror("rt_task_init(): GPSINS");
		exit(-1);
	}

	rt_make_hard_real_time();
	rt_task_make_periodic(rttGPSINS,
		rt_get_time()+nano2count(1000000000)*2,
		nano2count(1000000000));

#endif

       	/* 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);
		}
	  }
	}

	/* reset INS thread counter */
	iINSThreadCounter = 0;

	while(iRunThread == 1)
	{
		/* wait for message */ /* non blocking  if using RTAI*/
		iReturnValue = mq_receive(*ptmqdGPSINS,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0)
		{
			/* check value of errno */
			switch(errno)
			{
				case EAGAIN:
					/* return with no message - do nothing */
					break;
				default:
					perror("mq_recieve(): GPSINSThread");
					break;
			}

		}

// 		if(mMessage.iMessageType == CMD_SHUTDOWN)
// 		{
// 			break;
// 		}

		switch(mMessage.iMessageType)
		{
		  case CMD_RUNLOOP:
			break;

		  case CMD_SHUTDOWN:
			iRunThread = 0;
			break;

		  default:
			err("GPSINSThread Unknown Message");
			break;
		}

#ifdef USE_RTAI
		/* wait until its time to run */
		rt_task_wait_period();
#endif
//		fprintf(stdout,".\n");
		fprintf(stdout,"%s: GPSINS Counter: %d\n",MODULE_NAME,iINSThreadCounter);

		/* RUN PERIODIC TASKS HERE */

		/* get INS update from store */

		T_ECEF2NED(Pos_LLH[0],Pos_LLH[1],(double **)Tecef2ned);

		/* get GPS measurements */
		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++;
	  		} 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)
	  		{
	  			// 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
	  		{
	  		  fprintf(stdout,"NE: %d; ",shm->CurrentRANGE.usPRN[iIndex]);
	  		}
		}

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

		iNumberMeasurements = GPSRangeData.lNumberObservations;

		if(iNumberMeasurements < 4)
		{
			log("GPSINS Less than 4 Measurements.... ");
			if(iRunThread == 1)
			{
				log("GPSINS Continuing...");
				continue;
			} else
			{
				log("GPSINS Exiting..");
				break;
			}
		}

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

		EKF_z = gsl_vector_alloc(2*iNumberMeasurements);
		EKF_deltaz = gsl_vector_alloc(2*iNumberMeasurements);
		EKF_R = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		EKF_H = gsl_matrix_alloc(2*iNumberMeasurements,iNumberStates);
		tempV = gsl_matrix_alloc(2*iNumberMeasurements,iNumberStates);
		EKF_V = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		EKF_Vinv = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		EKF_K = gsl_matrix_alloc(iNumberStates,2*iNumberMeasurements);
		tempK = gsl_matrix_alloc(iNumberStates,2*iNumberMeasurements);
		tempKRKT = gsl_matrix_alloc(iNumberStates,2*iNumberMeasurements);
		KRKT = gsl_matrix_alloc(iNumberStates,iNumberStates);

		SVDWork = gsl_vector_alloc(2*iNumberMeasurements);
		SVD_S = gsl_vector_alloc(2*iNumberMeasurements);
		SVD_U = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		SVD_SinvMat = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		tempNN = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		SVD_V = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		SVD_Sinv = gsl_vector_alloc(2*iNumberMeasurements);

		/* now that all our memory is allocated, disable paging */
//		mlockall(MCL_CURRENT | MCL_FUTURE);

//		user_x = gsl_vector_get(EKF_x_hat,0);
//		user_y = gsl_vector_get(EKF_x_hat,1);
//		user_z = gsl_vector_get(EKF_x_hat,2);
//		user_t = gsl_vector_get(EKF_x_hat,15);

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

		WGS84_LLH2ECEF(Pos_LLH, UserPos);
		user_x = UserPos[0];
		user_y = UserPos[1];
		user_z = UserPos[2];
		user_t = UserClock[0];

//		user_xdot = gsl_vector_get(EKF_x_hat,3);
//		user_ydot = gsl_vector_get(EKF_x_hat,4);
//		user_zdot = gsl_vector_get(EKF_x_hat,5);
//		user_tdot = gsl_vector_get(EKF_x_hat,16);

//		UserVel[0] = user_xdot;
//		UserVel[1] = user_ydot;
//		UserVel[2] = user_zdot;
//		UserVel[3] = user_tdot;

		T_NED2ECEF(Pos_LLH[0], Pos_LLH[1], (double **)Tned2ecef);

		MatMul331(Tned2ecef, Vel_NED, UserVel);
		user_xdot = UserVel[0];
		user_ydot = UserVel[1];
		user_zdot = UserVel[2];
		user_tdot = UserClock[1];

		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);
		fprintf(stdout,"Organising Measurements\n");

		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);

			GPSOrbitPropagatorVelocities(dTransmissionTime,
						GPSRangeData.usPRN[iIndex],
						&GPSEphemData[GPSRangeData.usPRN[iIndex]],
						7500.0,
						SVVel_calc,
						SVAcc_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;

			sat_xdot = SVVel_calc[0];
			sat_ydot = SVVel_calc[1];
			sat_zdot = SVVel_calc[2];
			sat_tdot = SVVel_calc[3];

			// find the range
			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);

			/* calculate the range rate (relative velocity) to the SV */
			geo_vel_to_sat =  (sat_xdot - user_xdot)*(sat_x-user_x)
				  	+ (sat_ydot - user_ydot)*(sat_y-user_y)
					+ (sat_zdot - user_zdot)*(sat_z-user_z);

			rangerate = geo_vel_to_sat / range;

			EKF_H_E[iIndex][0] = -(sat_x - user_x)/range;
			EKF_H_E[iIndex][1] = -(sat_y - user_y)/range;
			EKF_H_E[iIndex][2] = -(sat_z - user_z)/range;

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

//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,3,-(sat_x - user_x)/range);
//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,4,-(sat_y - user_y)/range);
//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,5,-(sat_z - user_z)/range);
//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,16,1.0);

			gsl_vector_set(EKF_z,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]);

			TropoDelay[iIndex] = TropoDelayModel(SV_Elevation[iIndex],Pos_LLH[3]);

			/* set PR measurement */
			gsl_vector_set(EKF_deltaz,iIndex,
				GPSRangeData.dPseudorange[iIndex] - range
				- (user_t-sat_t)
				+ delta_pr_omegaedot
				- IonoDelay[iIndex]
				- TropoDelay[iIndex]);

//			fprintf(stdout,"%f %f %f %f %f %f\n",
//				range,
//				user_t,
//				sat_t,
//				delta_pr_omegaedot,
//				IonoDelay[iIndex],
//				TropoDelay[iIndex]);

			/* set doppler measurement */
			gsl_vector_set(EKF_deltaz,iIndex+iNumberMeasurements,
				-(double)GPSRangeData.fDoppler[iIndex]*GPS_L1_Wavelength
				- rangerate
				- (user_tdot));

		}  // end for i=1:NumberMeasurements

		// rotate H matrix to local
		MatMul333(Tecef2ned,EKF_H_E,EKF_H_LTP);

		for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
		{
			gsl_matrix_set(EKF_H,iIndex,0,EKF_H_LTP[iIndex][0]);
			gsl_matrix_set(EKF_H,iIndex,1,EKF_H_LTP[iIndex][1]);
			gsl_matrix_set(EKF_H,iIndex,2,EKF_H_LTP[iIndex][2]);
			gsl_matrix_set(EKF_H,iIndex,15,1.0);

			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,3,EKF_H_LTP[iIndex][0]);
			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,4,EKF_H_LTP[iIndex][1]);
			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,5,EKF_H_LTP[iIndex][2]);
			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,16,1.0);

		}

//		gsl_vector_fprintf(stdout,EKF_deltaz,"[%f]");
//		gsl_matrix_fprintf(stdout,EKF_H,"%f");

		fprintf(stdout,"Setting up PHI matrix\n");
		WGS84_calcRnRe(Pos_LLH[0],  &Rm, &Rp);
		Rmh = Rm+Pos_LLH[3];
		Rph = Rp+Pos_LLH[3];
		Rg = sqrt(Rmh*Rmh + Rph*Rph);

		/* kalman filter */
		gsl_matrix_set_zero(EKF_F);
		gsl_matrix_set(EKF_F,0,3,1.0/Rmh);
		gsl_matrix_set(EKF_F,1,4,1.0/(Rph*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,2,5,-1.0);
		gsl_matrix_set(EKF_F,0,2,-Vel_NED[0]/(Rg*Rg));
		gsl_matrix_set(EKF_F,1,0,Vel_NED[1]*sin(Pos_LLH[0])/(Rg*cos(Pos_LLH[0])*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,1,2,-Vel_NED[1]/(Rg*Rg*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,1,4,1.0/(Rg*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,3,0,-Vel_NED[1]*2.0*OMEGA_e*cos(Pos_LLH[0]) - Vel_NED[1]*Vel_NED[1]/(Rph*cos(Pos_LLH[0])*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,3,2,(Vel_NED[1]*Vel_NED[1] * tan(Pos_LLH[0]) - Vel_NED[0]*Vel_NED[2])/(Rg*Rg));
		gsl_matrix_set(EKF_F,3,3,Vel_NED[2]/Rg);
		gsl_matrix_set(EKF_F,3,4,-2.0*(OMEGA_e*sin(Pos_LLH[0]) + Vel_NED[1]*tan(Pos_LLH[0])/Rg));
		gsl_matrix_set(EKF_F,3,5,Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,4,0,2.0*OMEGA_e*(Vel_NED[0]*cos(Pos_LLH[0]) - Vel_NED[2]*sin(Pos_LLH[0])) +
								(Vel_NED[0]*Vel_NED[1]/(Rg*cos(Pos_LLH[0])*cos(Pos_LLH[0]))));
		gsl_matrix_set(EKF_F,4,2,-Vel_NED[1]*(Vel_NED[0]*tan(Pos_LLH[0]) - Vel_NED[1]*Vel_NED[2])/(Rg*Rg));
		gsl_matrix_set(EKF_F,4,3,2.0*OMEGA_e*sin(Pos_LLH[0]) + Vel_NED[1]*tan(Pos_LLH[0])/Rg);
		gsl_matrix_set(EKF_F,4,4,(Vel_NED[0]*tan(Pos_LLH[0])+Vel_NED[2])/Rg);
		gsl_matrix_set(EKF_F,4,5,2.0*OMEGA_e * cos(Pos_LLH[0]) + Vel_NED[1]/Rg);

		// velocity error due to tilt error
		gsl_matrix_set(EKF_F,3,7,-(Acc_NED[2] - LOCAL_GRAVITY));  // -fd
		gsl_matrix_set(EKF_F,3,8,(Acc_NED[1])); // fe
		gsl_matrix_set(EKF_F,4,6,(Acc_NED[2] - LOCAL_GRAVITY));  // fd
		gsl_matrix_set(EKF_F,4,8,-(Acc_NED[0])); // -fn
		gsl_matrix_set(EKF_F,5,6,-(Acc_NED[1])); // -fe
		gsl_matrix_set(EKF_F,5,7,(Acc_NED[0])); // fn

		gsl_matrix_set(EKF_F,5,0,2.0*OMEGA_e*Vel_NED[1]*sin(Pos_LLH[0]));
		gsl_matrix_set(EKF_F,5,2,(Vel_NED[0]*Vel_NED[0] + Vel_NED[1]*Vel_NED[1])/(Rg*Rg));  // note: gravity correction term missing
		gsl_matrix_set(EKF_F,5,3,-2.0*Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,5,4,-2.0*(OMEGA_e * cos(Pos_LLH[0]) + Vel_NED[1]/Rg));
		gsl_matrix_set(EKF_F,6,0,-OMEGA_e * sin(Pos_LLH[0]));
		gsl_matrix_set(EKF_F,6,2,-Vel_NED[1]/(Rg*Rg));
		gsl_matrix_set(EKF_F,7,2,Vel_NED[0] / (Rg*Rg));
		gsl_matrix_set(EKF_F,8,0,-OMEGA_e * cos(Pos_LLH[0]) - Vel_NED[1]/(Rg*cos(Pos_LLH[0])*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,8,2,Vel_NED[1]*tan(Pos_LLH[0])/(Rg*Rg));

		// tilt error due to velocity error
		gsl_matrix_set(EKF_F,6,4,1.0/Rg);
		gsl_matrix_set(EKF_F,7,3,-1.0/Rg);
		gsl_matrix_set(EKF_F,8,4,-tan(Pos_LLH[0])/Rg);

		// tilt error due to inertial rotation of the reference frame
		gsl_matrix_set(EKF_F,6,7,-OMEGA_e*sin(Pos_LLH[0]) - Vel_NED[1]*tan(Pos_LLH[0])/Rg);
		gsl_matrix_set(EKF_F,6,8,Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,7,6,OMEGA_e*sin(Pos_LLH[0]) + Vel_NED[1]*tan(Pos_LLH[0])/Rg);
		gsl_matrix_set(EKF_F,8,6,-Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,8,7,-OMEGA_e*cos(Pos_LLH[0]) - Vel_NED[1]/Rg);
		gsl_matrix_set(EKF_F,7,8,OMEGA_e*cos(Pos_LLH[0]) + Vel_NED[1]/Rg);

		for(iRowIndex=0;iRowIndex<3;iRowIndex++)
		{
			for(iColIndex=0;iColIndex<3;iColIndex++)
			{
				gsl_matrix_set(EKF_F,6+iRowIndex,9+iColIndex,-C_BN[iRowIndex][iColIndex]);
				gsl_matrix_set(EKF_F,3+iRowIndex,12+iColIndex,C_BN[iRowIndex][iColIndex]);
			}
		}

		gsl_matrix_set(EKF_F,9,9, -x_gyro_beta);
		gsl_matrix_set(EKF_F,10,10, -y_gyro_beta);
		gsl_matrix_set(EKF_F,11,11, -z_gyro_beta);
		gsl_matrix_set(EKF_F,12,12, -x_accel_beta);
		gsl_matrix_set(EKF_F,13,13, -y_accel_beta);
		gsl_matrix_set(EKF_F,14,14, -z_accel_beta);

		/*  PHI = I + F * dt */
		gsl_matrix_set_identity(EKF_PHI);
		gsl_matrix_scale (EKF_F, 1.0/GPS_RATE);
		gsl_matrix_add (EKF_PHI, EKF_F);

//		gsl_matrix_fprintf(stdout,EKF_PHI,"%f");

		fprintf(stdout,"Q-Matrix\n");
		/* Continuous Q-Matrix */
		gsl_matrix_set_identity(EKF_Q);

		/* Discrete Qd = PHI * G * Q * G' * PHI' */
		// TODO: Qd

		fprintf(stdout,"R-Matrix\n");

		/* R - Matrix */
		gsl_matrix_set_identity(EKF_R);

		for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
		{
			gsl_matrix_set(EKF_R,iIndex,iIndex,3.0);
			gsl_matrix_set(EKF_R,iIndex+iNumberMeasurements,iIndex+iNumberMeasurements,0.3);
		}

		/* prediction step */
		/* x- = PHI * x+ */
		/* z- = H * x- */
		/* P- = PHI * P+ * PHI' + Q */
		gsl_matrix_memcpy(EKF_P_minus,EKF_Q);
		gsl_matrix_set_zero(tempP);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,EKF_PHI,EKF_P,1.0, tempP);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,tempP,EKF_PHI,1.0,EKF_P_minus);

		//V = H * P_minus * H' + R;
		fprintf(stdout,"HPH'+R\n");

		gsl_matrix_set_zero (tempV);
		gsl_matrix_set_zero(EKF_V);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, EKF_H, EKF_P_minus, 1.0, tempV);
		gsl_matrix_memcpy(EKF_V,EKF_R);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0,tempV, EKF_H, 1.0,EKF_V);	 //this adds V = V + A*B

		/* calculate inv(EKF_V) */
		/*

		*/
		fprintf(stdout,"inv(V)\n");

		iReturnValue =  gsl_linalg_SV_decomp (EKF_V,SVD_V,SVD_S,SVDWork);
		gsl_matrix_memcpy(SVD_U,EKF_V);

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

		gsl_matrix_set_zero (SVD_SinvMat);
		for(iIndex=0;iIndex<2*iNumberMeasurements;iIndex++)
		{
			gsl_matrix_set(SVD_SinvMat,iIndex,iIndex,gsl_vector_get(SVD_Sinv,iIndex));
		}

		// inv(S)*UT
		gsl_matrix_set_zero (tempNN);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, SVD_SinvMat, SVD_U, 1.0, tempNN);

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

		fprintf(stdout,"PH'*inv(V)\n");

		//K = P_minus * H' * inv(V);
		gsl_matrix_set_zero (tempK);
		gsl_matrix_set_zero(EKF_K);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, EKF_P_minus, EKF_H, 1.0, tempK);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0,tempK, EKF_Vinv, 1.0,EKF_K);

		// x_hat_out = K * (z);
		gsl_vector_set_zero(EKF_x_hat_out);

		fprintf(stdout,"Kz\n");
		gsl_blas_dgemv (CblasNoTrans, 1.0, EKF_K, EKF_deltaz, 1.0, EKF_x_hat_out);

		// %symmetric form of the P-update equation to ward off divergence p347
		//%Brown and Hwang
		// P_out = (eye(NumberStates) - K*H)*P_minus*(eye(NumberStates)-K*H)' + K*R*K';

		//K*H
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0,EKF_K, EKF_H, 1.0,KH);

		//(eye(NumberStates) - K*H)
		gsl_matrix_set_identity(I_17by17);
		gsl_matrix_memcpy(IminusKH,I_17by17);
		gsl_matrix_sub(IminusKH, KH);

		//(eye(NumberStates) - K*H)*P_minus
		gsl_matrix_set_zero(KHPminus);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, IminusKH, EKF_P_minus, 1.0, KHPminus);

		//(eye(NumberStates) - K*H)*P_minus*(eye(NumberStates)-K*H)'
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, KHPminus, IminusKH, 1.0, EKF_P);

		//K*R*K'
		gsl_matrix_set_zero (tempKRKT);
		gsl_matrix_set_zero(KRKT);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, EKF_K, EKF_R, 1.0, tempKRKT);

		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0,tempKRKT, EKF_K, 1.0,KRKT);
		gsl_matrix_sub(EKF_P, KRKT);

		// apply correction
		Pos_LLH[0] -= gsl_vector_get(EKF_x_hat_out,0);
		Pos_LLH[1] -= gsl_vector_get(EKF_x_hat_out,1);
		Pos_LLH[2] -= gsl_vector_get(EKF_x_hat_out,2);
		Vel_NED[0] -= gsl_vector_get(EKF_x_hat_out,3);
		Vel_NED[1] -= gsl_vector_get(EKF_x_hat_out,4);
		Vel_NED[2] -= gsl_vector_get(EKF_x_hat_out,5);

		AccBias[0] -= gsl_vector_get(EKF_x_hat_out,9);
		AccBias[1] -= gsl_vector_get(EKF_x_hat_out,10);
		AccBias[2] -= gsl_vector_get(EKF_x_hat_out,11);

		GyroBias[0] -= gsl_vector_get(EKF_x_hat_out,12);
		GyroBias[1] -= gsl_vector_get(EKF_x_hat_out,13);
		GyroBias[2] -= gsl_vector_get(EKF_x_hat_out,14);

		// attitude
		del_alpha = -gsl_vector_get(EKF_x_hat_out,6);
		del_beta = -gsl_vector_get(EKF_x_hat_out,7);
		del_gamma = -gsl_vector_get(EKF_x_hat_out,8);

		del_att_skew[0][0] = 1.0;
		del_att_skew[0][1] = -del_gamma;
		del_att_skew[0][2] = del_beta;
		del_att_skew[1][0] = del_gamma;
		del_att_skew[1][1] = 1.0;
		del_att_skew[1][2] = -del_alpha;
		del_att_skew[2][0] = -del_beta;
		del_att_skew[2][1] = del_alpha;
		del_att_skew[2][2] = 1.0;

		/* C_BN = (eye(3,3) + del_att_skew) * C_BN */
		MatMul333(del_att_skew,C_BN,C_BN_new);
		memcpy(C_BN_new,C_BN,sizeof(C_BN));

		// clock
		UserClock[0] -= gsl_vector_get(EKF_x_hat_out,15);
		UserClock[1] -= gsl_vector_get(EKF_x_hat_out,16);

		/* re-enable paging */
//		munlockall();

		log("Result: \n");
		fprintf(stdout,"Pos: %f %f %f\n",
			Pos_LLH[0]*RAD2DEG,
			Pos_LLH[1]*RAD2DEG,
			Pos_LLH[2]);
		fprintf(stdout,"Vel: %f %f %f\n",
			Vel_NED[0],
			Vel_NED[1],
			Vel_NED[2]);
		fprintf(stdout,"Clock: %f %f\n",
			UserClock[0],UserClock[1]);

		fprintf(stdout,"X-out\n");
		gsl_vector_fprintf(stdout,EKF_x_hat_out,"[%f]");

//		fprintf(stdout,"delta Z\n");
//		gsl_vector_fprintf(stdout,EKF_deltaz,"[%f]");

		log("Releasing Matrices");

		gsl_vector_free(EKF_z);
		gsl_vector_free(EKF_deltaz);
		gsl_matrix_free(EKF_H);
		gsl_matrix_free(EKF_R);
		gsl_matrix_free(tempV);
		gsl_matrix_free(EKF_V);
		gsl_matrix_free(tempNN);
		gsl_vector_free(SVD_S);
		gsl_vector_free(SVD_Sinv);
		gsl_matrix_free(SVD_SinvMat);
		gsl_matrix_free(SVD_V);
		gsl_vector_free(SVDWork);
		gsl_matrix_free(EKF_K);
		gsl_matrix_free(tempK);
		gsl_matrix_free(KRKT);
		gsl_matrix_free(tempKRKT);

	}

	/* exit */
	fprintf(stdout,"%s GPS-INS Thread Exiting\n",MODULE_NAME);

	gsl_matrix_free(EKF_PHI);
	gsl_matrix_free(EKF_F);
	gsl_matrix_free(EKF_P_minus);
	gsl_matrix_free(tempP);
	gsl_matrix_free(EKF_P);
	gsl_matrix_free(I_17by17);
	gsl_vector_free(EKF_x_hat);
	gsl_matrix_free(EKF_Q);
	gsl_vector_free(temp1);
	gsl_matrix_free(KH);
	gsl_matrix_free(IminusKH);
	gsl_matrix_free(KHPminus);

#ifdef USE_RTAI
	rt_make_soft_real_time();

	if(rttGPSINS)
	{
		rt_task_delete(rttGPSINS);
	}
	stop_rt_timer();
#endif

	pthread_exit(NULL);
}


#include <stdio.h>       /* standard I/O routines                 */
#include <stdlib.h>
#include <string.h>
#include <stdio.h>       /* standard I/O routines                 */
#include <omp.h>
#include <mpi.h>

#include <sys/time.h>
#include "utils.h"
//#include <cblas.h>
#ifdef PAPI
#include "cscs_papi.h"
#endif

#define NN 8000;
#define NTIMES 10

#if 0
extern "C"{
void dgemm_ (char *transa,
		char *transb,
		int *m, int *n, int *k,
		double *alpha, double *A, int *lda,
		double *B, int *ldb,
		double *beta, double *C, int *ldc) ;
}
#endif

double mysecond()
{
	struct timeval tp;
	struct timezone tzp;
	int i;

	i = gettimeofday(&tp,&tzp);
	return ( (double) tp.tv_sec + (double) tp.tv_usec * 1.e-6 );
}

int main (int argc, char *argv[])
{

	int N = NN;
	int M = NN;
	int K = NN;
	int ii;

	/* Find out my identity in the default communicator */

	int Ncpu = 0;

	if ( argc == 2 )
	{
		N    = atoi(argv[1]);
		M = N;
		K = N;
	}

	double  alpha =  1.;
	double  beta  = -1.;

	int lda    = M;
	int ldb    = K;
	int ldc    = M;

	int size_A = K*lda;
	int size_B = N*ldb;
	int size_C = N*ldc;

	double *A  = (double*) malloc(sizeof(double)*size_A);
	if (A == 0) printf("Could not allocate A.\n");

	double *B  = (double*) malloc(sizeof(double)*size_B);
	if (B == 0) printf("Could not allocate B.\n");

	double *C  = (double*) malloc(sizeof(double)*size_C);
	if (C == 0) printf("Could not allocate C.\n");

	double *Cg = (double*) malloc(sizeof(double)*size_C);
	if (Cg == 0) printf("Could not allocate Cg.\n");

	fill(A,  size_A,  31.);
	eye (B,     ldb,   N );
	fill(C,  size_C,  31.);
	fill(Cg, size_C,  31.);

	int t;

	int nthreads, tid, procs, maxthreads, inpar, dynamic, nested;

	char transa = 'n';
	char transb = 'n';

	/* Get environment information */
#ifdef _OPENMP
	procs      = omp_get_num_procs();
	nthreads   = omp_get_num_threads();
	maxthreads = omp_get_max_threads();
	inpar      = omp_in_parallel();
	dynamic    = omp_get_dynamic();
	nested     = omp_get_nested();
	/* Print environment information */
	printf("Number of processors          = %d\n", procs);
	printf("Number of threads             = %d\n", nthreads);
	printf("Max threads                   = %d\n", maxthreads);
	printf("In parallel?                  = %d\n", inpar);
	printf("Dynamic threads enabled?      = %d\n", dynamic);
	printf("Nested parallelism supported? = %d\n", nested);
#endif

	double otime = 0.;
	otime -= mysecond();

	for (ii = 0; ii < NTIMES; ++ii)
		dgemm_(&transa, &transb,
				&M, &N, &K,
				&alpha, A, &lda,
				B, &ldb,
				&beta, Cg, &ldc);

	otime += mysecond();
	otime /= NTIMES;

	printf("Size = %d, Gflops Max: %f  Min: %f  Avg:  %f\n", N, 2.*M*N*K/otime/1e9, 2.*M*N*K/otime/1e9, 2.*M*N*K/otime/1e9);

	free(A);
	free(B);
	free(C);
	free(Cg);

	exit(0);
}


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/**
 * \file rk_6_4.c
 * \brief Source file to optimize Runge-Kutta 6 steps 4th order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_6_3.h"

#define DEBUG_RK_6_3 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 6 steps 3rd order Runge-Kutta
 * method.
 */
int
rk_tb_6_3 (Optimize * optimize) ///< Optimize struct.
{
  long double A[3], B[3], C[3], D[3];
  long double *tb, *r;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t6 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  t5 (tb) = r[10];
  b51 (tb) = r[11];
  b52 (tb) = r[12];
  b53 (tb) = r[13];
  b54 (tb) = r[14];
  b65 (tb) = r[15];
  b64 (tb) = r[16];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = 0.5L - b64 (tb) * t4 (tb) - b65 (tb) * t5 (tb);
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = 1.L / 3.L - b64 (tb) * sqr (t4 (tb)) - b65 (tb) * sqr (t5 (tb));
  A[2] = 0.L;
  B[2] = b21 (tb) * t1 (tb);
  C[2] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[2] = 1.L / 6.L - b64 (tb) * (b41 (tb) * t1 (tb) + b42 (tb) * t2 (tb)
                                 + b43 (tb) * t3 (tb))
    - b65 (tb) * (b51 (tb) * t1 (tb) + b52 (tb) * t2 (tb)
                  + b53 (tb) * t3 (tb) + b54 (tb) * t4 (tb));
  solve_3 (A, B, C, D);
  if (isnan (D[0]) || isnan (D[1]) || isnan (D[2]))
    return 0;
  b63 (tb) = D[2];
  b62 (tb) = D[1];
  b61 (tb) = D[0];
  rk_b_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 3rd order, 4th order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_6_3t (Optimize * optimize)        ///< Optimize struct.
{
  long double A[4], B[4], C[4], D[4], E[4];
  long double *tb, *r;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t6 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  t5 (tb) = r[10];
  b51 (tb) = r[11];
  b52 (tb) = r[12];
  b53 (tb) = r[13];
  b54 (tb) = r[14];
  b65 (tb) = r[15];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = t4 (tb);
  E[0] = 0.5L - b65 (tb) * t5 (tb);
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = D[0] * t4 (tb);
  E[1] = 1.L / 3.L - b65 (tb) * sqr (t5 (tb));
  A[2] = A[1] * t1 (tb);
  B[2] = B[1] * t2 (tb);
  C[2] = C[1] * t3 (tb);
  D[2] = D[1] * t4 (tb);
  E[2] = 0.25L - b65 (tb) * sqr (t5 (tb)) * t5 (tb);
  A[3] = 0.L;
  B[3] = b21 (tb) * t1 (tb);
  C[3] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[3] = b41 (tb) * t1 (tb) + b42 (tb) * t2 (tb) + b43 (tb) * t3 (tb);
  E[3] = 1.L / 6.L - b65 (tb) * (b51 (tb) * t1 (tb) + b52 (tb) * t2 (tb)
                                 + b53 (tb) * t3 (tb) + b54 (tb) * t4 (tb));
  solve_4 (A, B, C, D, E);
  if (isnan (E[0]) || isnan (E[1]) || isnan (E[2]) || isnan (E[3]))
    return 0;
  b64 (tb) = E[3];
  b63 (tb) = E[2];
  b62 (tb) = E[1];
  b61 (tb) = E[0];
  rk_b_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 2nd-3rd order Runge-Kutta
 * pair.
 */
int
rk_tb_6_3p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3p: start\n");
#endif
  if (!rk_tb_6_3 (optimize))
    return 0;
  tb = optimize->coefficient;
  e51 (tb) = 0.5L / t1 (tb);
  e52 (tb) = e53 (tb) = 0.L;
  rk_e_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 2nd-3rd order, 3rd-4th order
 * in equations depending only in time, Runge-Kutta pair.
 */
int
rk_tb_6_3tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3tp: start\n");
#endif
  if (!rk_tb_6_3t (optimize))
    return 0;
  tb = optimize->coefficient;
  e63 (tb) = e64 (tb) = 0.L;
  e62 (tb) = (1.L / 3.L - 0.5L * t1 (tb)) / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (e62 (tb)))
    return 0;
  e61 (tb) = (0.5L - e52 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (e61 (tb)))
    return 0;
  rk_e_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3tp: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 6 steps 3rd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 6 steps 3rd order, 4th
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3t: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3t: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 6 steps 2nd-3rd order
 * Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3p (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3p: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (e60 (tb) < 0.L)
    o += e60 (tb);
  if (e61 (tb) < 0.L)
    o += e61 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3p: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3p: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 6 steps 2nd-3rd order,
 * 3rd-4th order in equations depending only in time, Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3tp (RK * rk) ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3tp: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (e60 (tb) < 0.L)
    o += e60 (tb);
  if (e61 (tb) < 0.L)
    o += e61 (tb);
  if (e62 (tb) < 0.L)
    o += e62 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3tp: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3tp: end\n");
#endif
  return o;
}


#include <unistd.h>
#include <stdio.h>
#include <gsl/gsl_math.h>

int main (void) {
    for ( ; ; ) {
    double x = 5.0;
    double y = gsl_expm1(100.0);
    printf ("J0(%g) = %.18e\n", x, y);
    sleep(2);
    }
    return 0;
}


#include <pygsl/utils.h>
#include <pygsl/error_helpers.h>
#include <pygsl/block_helpers.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_nan.h>

#ifndef IMPORTALL
static PyObject *module=NULL;
#endif

typedef  int (array_p_evaluator_iid_ad)(int nmin, int nmax, double x, double * result_array);
static PyObject*
PyGSL_sf_array_evaluator_legendre_iid_ad(PyObject *self, PyObject *args,
					 array_p_evaluator_iid_ad * eval)
{
     PyArrayObject *result = NULL;
     int lmax=0, m=0, dimension = 0, ret;
     double x=0, *data=NULL;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "iid", &lmax, &m, &x)){
	  return NULL;
     }
     if(m < 0){
	  PyErr_SetString(PyExc_ValueError,
			  "Nmin must be bigger than 0!");
	  return NULL;
     }
     if(lmax < m){
	  PyErr_SetString(PyExc_ValueError,
			  "Nmax must be bigger or equal to nmin!");
     }
     dimension = gsl_sf_legendre_array_size(lmax, m);
     result = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result == NULL)
	  return NULL;

     data = (double *) result->data;
     ret =  eval(lmax, m, x, data);
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     FUNC_MESS_END();
     return (PyObject *) result;

 fail:
     Py_XDECREF(result);
     return NULL;
}

static PyObject*
PyGSL_sf_array_evaluator_iid_ad(PyObject *self, PyObject *args,
				array_p_evaluator_iid_ad * eval)
{
     PyArrayObject *result = NULL;
     int nmin=0, nmax=0, dimension = 0, ret;
     double x=0, *data=NULL;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "iid", &nmin, &nmax, &x)){
	  return NULL;
     }
     if(nmin < 0){
	  PyErr_SetString(PyExc_ImportError,
			  "Nmin must be bigger than 0!");
	  return NULL;
     }
     if(nmax < nmin){
	  PyErr_SetString(PyExc_ImportError,
			  "Nmax must be bigger or equal to nmin!");
     }
     dimension = nmax - nmin + 1; /* Goes form nmin to nmax, both included */
     result = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result == NULL)
	  return NULL;

     data = (double *) result->data;
     ret =  eval(nmin, nmax, x, data);
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     FUNC_MESS_END();
     return (PyObject *) result;

 fail:
     Py_XDECREF(result);
     return NULL;
}

typedef  int (array_p_evaluator_id_ad)(int nmax, double x, double * result_array);

static PyObject*
PyGSL_sf_array_evaluator_id_ad(PyObject *self, PyObject *args, array_p_evaluator_id_ad * eval)
{
     PyArrayObject *result = NULL;
     int nmin=0, nmax=0, dimension = 0, ret;
     double x=0, *data=NULL;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "id", &nmax, &x)){
	  return NULL;
     }
     if(nmin < 0){
	  PyErr_SetString(PyExc_ImportError,
			  "Nmin must be bigger than 0!");
	  return NULL;
     }
     dimension = nmax - nmin + 1; /* Goes form nmin to nmax, both included */
     result = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result == NULL)
	  return NULL;

     data = (double *) result->data;
     ret =  eval(nmax, x, data);
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     FUNC_MESS_END();
     return (PyObject *) result;

 fail:
     Py_XDECREF(result);
     return NULL;
}

typedef  int (array_p_evaluator_idd_ad)(int nmax, double x1, double x2, double * result_array);
static PyObject*
PyGSL_sf_array_evaluator_idd_ad(PyObject *self, PyObject *args, array_p_evaluator_idd_ad * eval)
{
     PyArrayObject *result = NULL;
     int nmin=0, nmax=0, dimension = 0, ret;
     double x=0, x1=0, *data=NULL;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "idd", &nmax, &x, &x1)){
	  return NULL;
     }
     if(nmin < 0){
	  PyErr_SetString(PyExc_ImportError,
			  "Nmin must be bigger than 0!");
	  return NULL;
     }
     dimension = nmax - nmin + 1; /* Goes form nmin to nmax, both included */
     result = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result == NULL)
	  return NULL;

     data = (double *) result->data;
     ret =  eval(nmax, x, x1, data);
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     FUNC_MESS_END();
     return (PyObject *) result;

 fail:
     Py_XDECREF(result);
     return NULL;
}
typedef  int (array_p_evaluator_did_ad)( double x1, int nmax, double x2, double * result_array);
static PyObject*
PyGSL_sf_array_evaluator_did_ad(PyObject *self, PyObject *args, array_p_evaluator_did_ad * eval)
{
     PyArrayObject *result = NULL;
     int nmin=0, nmax=0, dimension = 0, ret;
     double x=0, x1=0, *data=NULL;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "did",&x, &nmax, &x1)){
	  return NULL;
     }
     dimension = nmax - nmin + 1; /* Goes form nmin to nmax, both included */
     result = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result == NULL)
	  return NULL;

     data = (double *) result->data;
     ret =  eval(nmax, x, x1, data);
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     FUNC_MESS_END();
     return (PyObject *) result;

 fail:
     Py_XDECREF(result);
     return NULL;
}

typedef int (array_p_evaluator_didd_add)(double , int , double , double , double * array, double*);
static PyObject*
PyGSL_sf_array_evaluator_didd_add(PyObject *self, PyObject *args, array_p_evaluator_didd_add * eval)
{
     PyArrayObject *result = NULL;
     int nmin=0, nmax=0, dimension = 0, ret;
     double x=0, x1=0, *data=NULL, l_min, exponent;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "didd", &l_min, &nmax, &x, &x1)){
	  return NULL;
     }
     if(nmin < 0){
	  PyErr_SetString(PyExc_ImportError,
			  "Nmin must be bigger than 0!");
	  return NULL;
     }
     dimension = nmax - nmin + 1; /* Goes form nmin to nmax, both included */
     result = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result == NULL)
	  return NULL;

     data = (double *) result->data;
     ret =  eval(l_min, nmax, x, x1, data, &exponent);
     FUNC_MESS_END();
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     return Py_BuildValue("Od",result,exponent);
 fail:
     Py_XDECREF(result);
     return NULL;
}

typedef int (array_p_evaluator_didd_addadd)(double , int , double , double , double * array1, double*, double *array2, double*);
static PyObject*
PyGSL_sf_array_evaluator_didd_addadd(PyObject *self, PyObject *args, array_p_evaluator_didd_addadd * eval)
{
     PyArrayObject *result1 = NULL,*result2 = NULL;
     int nmin=0, nmax=0, dimension = 0, ret;
     double x=0, x1=0, *data1=NULL, *data2=NULL, l_min, exponent1,exponent2;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "didd", &l_min, &nmax, &x, &x1)){
	  return NULL;
     }
     if(nmin < 0){
	  PyErr_SetString(PyExc_ImportError,
			  "Nmin must be bigger than 0!");
	  return NULL;
     }
     dimension = nmax - nmin + 1; /* Goes form nmin to nmax, both included */
     result1 = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result1 == NULL)
	  goto fail;
     result2 = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result2 == NULL)
	  goto fail;

     data1 = (double *) result1->data;
     data2 = (double *) result2->data;

     ret =  eval(l_min, nmax, x, x1, data1, &exponent1, data2, &exponent2);
     FUNC_MESS_END();
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     return Py_BuildValue("OdOd",result1,exponent1,result2,exponent2);
 fail:
     Py_XDECREF(result1);
     Py_XDECREF(result2);
     return NULL;
}

typedef int (array_p_evaluator_didd_adadadaddd)(double , int , double , double , double * a1, double * a2, double * a3, double * a4,
						double*, double*);
static PyObject*
PyGSL_sf_array_evaluator_didd_adadadaddd(PyObject *self, PyObject *args, array_p_evaluator_didd_adadadaddd * eval)
{
     PyArrayObject *result1 = NULL,*result2 = NULL, *result3=NULL,*result4=NULL;
     int nmin=0, nmax=0, dimension = 0, ret;
     double x=0, x1=0, l_min, exponent1, exponent2;

     FUNC_MESS_BEGIN();
     if (!PyArg_ParseTuple(args, "didd", &l_min, &nmax, &x, &x1)){
	  return NULL;
     }
     if(nmin < 0){
	  PyErr_SetString(PyExc_ImportError,
			  "Nmin must be bigger than 0!");
	  return NULL;
     }
     dimension = nmax - nmin + 1; /* Goes form nmin to nmax, both included */
     result1 = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result1 == NULL)
	  goto fail;
     result2 = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result2 == NULL)
	  goto fail;
     result3 = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result3 == NULL)
	  goto fail;
     result4 = (PyArrayObject *) PyArray_FromDims(1, &dimension, PyArray_DOUBLE);
     if(result4 == NULL)
	  goto fail;

     ret =  eval(l_min, nmax, x, x1, (double *) result1->data, (double *)result2->data,
		 (double *) result3->data, (double *)result4->data, &exponent1, &exponent2);
     FUNC_MESS_END();
     if(PyGSL_ERROR_FLAG(ret) != GSL_SUCCESS)
	  goto fail;

     return Py_BuildValue("OOOOdd",result1,result2,result3,result4,exponent1, exponent2);
 fail:
     Py_XDECREF(result1);
     Py_XDECREF(result2);
     Py_XDECREF(result3);
     Py_XDECREF(result4);
     return NULL;
}

#define SF_ARRAY(name, function)                                           \
static PyObject* sf_ ## name      (PyObject *self, PyObject *args)         \
{                                                                          \
     PyObject * tmp;                                                       \
     FUNC_MESS_BEGIN();                                                    \
     tmp =  PyGSL_sf_array_evaluator_ ## function (self, args, gsl_sf_ ## name); \
     if (tmp == NULL){                                                     \
	  PyGSL_add_traceback(module, __FILE__, __FUNCTION__,  __LINE__);  \
     }                                                                     \
     FUNC_MESS_END();                                                      \
     return tmp;                                                           \
}

SF_ARRAY(bessel_Jn_array, iid_ad);
SF_ARRAY(bessel_Yn_array, iid_ad);
SF_ARRAY(bessel_In_array, iid_ad);
SF_ARRAY(bessel_Kn_array, iid_ad);
SF_ARRAY(bessel_Kn_scaled_array, iid_ad);
SF_ARRAY(bessel_jl_array, id_ad);
SF_ARRAY(bessel_jl_steed_array, id_ad);
SF_ARRAY(bessel_yl_array, id_ad);
SF_ARRAY(bessel_il_scaled_array, id_ad);
SF_ARRAY(bessel_kl_scaled_array, id_ad);
SF_ARRAY(gegenpoly_array, idd_ad);
SF_ARRAY(legendre_H3d_array, idd_ad);
SF_ARRAY(coulomb_wave_F_array, didd_add);
SF_ARRAY(coulomb_wave_sphF_array, didd_add);
SF_ARRAY(coulomb_wave_FG_array, didd_addadd);
SF_ARRAY(coulomb_wave_FGp_array, didd_adadadaddd);
SF_ARRAY(coulomb_CL_array, did_ad);
SF_ARRAY(legendre_Pl_array, id_ad);
SF_ARRAY(legendre_Plm_array, legendre_iid_ad);
SF_ARRAY(legendre_sphPlm_array, legendre_iid_ad);

static PyMethodDef sf_array_functions[] = {
     {"bessel_Jn_array",         (PyCFunction) sf_bessel_Jn_array,         METH_VARARGS, NULL},
     {"bessel_Yn_array",         (PyCFunction) sf_bessel_Yn_array,         METH_VARARGS, NULL},
     {"bessel_In_array",         (PyCFunction) sf_bessel_In_array,         METH_VARARGS, NULL},
     {"bessel_Kn_array",         (PyCFunction) sf_bessel_Kn_array,         METH_VARARGS, NULL},
     {"bessel_Kn_scaled_array",  (PyCFunction) sf_bessel_Kn_scaled_array,  METH_VARARGS, NULL},
     {"bessel_jl_array",         (PyCFunction) sf_bessel_jl_array,         METH_VARARGS, NULL},
     {"bessel_jl_steed_array",   (PyCFunction) sf_bessel_jl_steed_array,   METH_VARARGS, NULL},
     {"bessel_yl_array",         (PyCFunction) sf_bessel_yl_array,         METH_VARARGS, NULL},
     {"bessel_il_scaled_array",  (PyCFunction) sf_bessel_il_scaled_array,  METH_VARARGS, NULL},
     {"bessel_kl_scaled_array",  (PyCFunction) sf_bessel_kl_scaled_array,  METH_VARARGS, NULL},
     {"gegenpoly_array",         (PyCFunction) sf_gegenpoly_array,         METH_VARARGS, NULL},
     {"legendre_H3d_array",      (PyCFunction) sf_legendre_H3d_array,      METH_VARARGS, NULL},
     {"coulomb_wave_F_array",    (PyCFunction) sf_coulomb_wave_F_array,    METH_VARARGS, NULL},
     {"coulomb_wave_sphF_array", (PyCFunction) sf_coulomb_wave_sphF_array, METH_VARARGS, NULL},
     {"coulomb_wave_FG_array",   (PyCFunction) sf_coulomb_wave_FG_array,   METH_VARARGS, NULL},
     {"coulomb_wave_FGp_array",  (PyCFunction) sf_coulomb_wave_FGp_array,  METH_VARARGS, NULL},
     {"coulomb_CL_array",        (PyCFunction) sf_coulomb_CL_array,        METH_VARARGS, NULL},
     {"legendre_Pl_array",       (PyCFunction) sf_legendre_Pl_array,       METH_VARARGS, NULL},
     {"legendre_Plm_array",      (PyCFunction) sf_legendre_Plm_array,      METH_VARARGS, NULL},
     {"legendre_sphPlm_array",   (PyCFunction) sf_legendre_sphPlm_array,   METH_VARARGS, NULL},
     {NULL, NULL, 0}
};

#ifndef IMPORTALL

DL_EXPORT(void) initsfarray(void)
{
     module = Py_InitModule("sfarray", sf_array_functions);
     import_array();
       init_pygsl();

}
#endif


// @Copyright 2007 Kristjan Haule
//
#include <cstdio>
#include <cstdlib>
#include <gsl/gsl_rng.h>
#include <map>

class RanGSL{
  const gsl_rng_type *T;
  gsl_rng *r;
public:
  RanGSL(int idum)
  {
    gsl_rng_env_setup();
    //    T = gsl_rng_default;
    //    T = gsl_rng_taus;
    T = gsl_rng_ranlux389;
    r = gsl_rng_alloc (T);
    gsl_rng_set(r, idum);
  }
  double operator()()
  { return gsl_rng_uniform (r);}
  ~RanGSL()
  {gsl_rng_free (r);}
};

class dRand48{
public:
  dRand48(int idum){srand48(idum);}
  double operator()(){
    return drand48();
  }
};

class Ran0{
  long idum;
public:
  Ran0(long idum_): idum(idum_){};
  double operator()(){
    // Minimal random number generator of Park and Miller. Returns a uniform random deviate
    // between 0.0 and 1.0. Set or reset idum to any integer value (except the unlikely value MASK)
    // to initialize the sequence; idum must not be altered between calls for successive deviates in
    // a sequence.
    static const int IA=16807;
    static const int IM=2147483647;
    static const double AM=(1.0/IM);
    static const int IQ=127773;
    static const int IR=2836;
    static const int MASK=123459876;
    static long idum;
    long k;
    float ans;
    idum ^= MASK; //XORing with MASK allows use of zero and other simple bit patterns for idum.
    k=idum/IQ;
    idum = IA*(idum-k*IQ)-IR*k; // Compute idum=(IA*idum) % IM without over
    if (idum < 0) idum += IM; // flows by Schrages method.
    ans=AM*idum; // Convert idum to a floating result.
    idum ^= MASK; // Unmask before return.
    return ans;
  }
};


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for functions windowing and computing FFT/IFFT of time/frequency series.
 *
 */

#ifndef __GENERATELLVTD_H__
#define __GENERATELLVTD_H__ 1

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#define _XOPEN_SOURCE 500

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "waveform.h"
#include "LISAgeometry.h"

/********************************** Structures ******************************************/

typedef enum GenWavetag {
  hphcTD,
  hphcFD,
  hlm
} GenWavetag;

/* Parameters for the generation of a LISA waveform (in the form of a list of modes) */
typedef struct tagGenLLVTDparams {
  double lambda;             /* first angle for the position in the sky (rad, default 0) */
  double beta;               /* second angle for the position in the sky (rad, default 0) */
  double polarization;       /* polarization angle (rad, default 0) */
  int tagnetwork;            /* Tag selecting the desired detector network */
  int nsamplesinfile;        /* Number of lines of input file */
  int binaryin;              /* Tag for loading the data in gsl binary form instead of text (default false) */
  int binaryout;             /* Tag for outputting the data in gsl binary form instead of text (default false) */
  char indir[256];           /* Path for the input directory */
  char infile[256];          /* Path for the input file */
  char outdir[256];          /* Path for the output directory */
  char outfile[256];         /* Path for the output file */
} GenLLVTDparams;

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _GENERATELLVTD_H */


/////////////////////////////////////////////////////////////////////
// = NMatrix
//
// A linear algebra library for scientific computation in Ruby.
// NMatrix is part of SciRuby.
//
// NMatrix was originally inspired by and derived from NArray, by
// Masahiro Tanaka: http://narray.rubyforge.org
//
// == Copyright Information
//
// SciRuby is Copyright (c) 2010 - 2014, Ruby Science Foundation
// NMatrix is Copyright (c) 2012 - 2014, John Woods and the Ruby Science Foundation
//
// Please see LICENSE.txt for additional copyright notices.
//
// == Contributing
//
// By contributing source code to SciRuby, you agree to be bound by
// our Contributor Agreement:
//
// * https://github.com/SciRuby/sciruby/wiki/Contributor-Agreement
//
// == gemm.h
//
// Header file for interface with ATLAS's CBLAS gemm functions and
// native templated version of LAPACK's gemm function.
//

#ifndef GEMM_H
# define GEMM_H

#if !(defined(HAVE_CBLAS_H) || defined(HAVE_ATLAS_CBLAS_H))
#include "cblas_enums.h"
#endif

extern "C" { // These need to be in an extern "C" block or you'll get all kinds of undefined symbol errors.
#if defined HAVE_CBLAS_H
  #include <cblas.h>
#elif defined HAVE_ATLAS_CBLAS_H
  #include <atlas/cblas.h>
#endif
}

namespace nm { namespace math {
/*
 * GEneral Matrix Multiplication: based on dgemm.f from Netlib.
 *
 * This is an extremely inefficient algorithm. Recommend using ATLAS' version instead.
 *
 * Template parameters: LT -- long version of type T. Type T is the matrix dtype.
 *
 * This version throws no errors. Use gemm<DType> instead for error checking.
 */
template <typename DType>
inline void gemm_nothrow(const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
                 const DType* alpha, const DType* A, const int lda, const DType* B, const int ldb, const DType* beta, DType* C, const int ldc)
{

  typename LongDType<DType>::type temp;

  // Quick return if possible
  if (!M or !N or ((*alpha == 0 or !K) and *beta == 1)) return;

  // For alpha = 0
  if (*alpha == 0) {
    if (*beta == 0) {
      for (int j = 0; j < N; ++j)
        for (int i = 0; i < M; ++i) {
          C[i+j*ldc] = 0;
        }
    } else {
      for (int j = 0; j < N; ++j)
        for (int i = 0; i < M; ++i) {
          C[i+j*ldc] *= *beta;
        }
    }
    return;
  }

  // Start the operations
  if (TransB == CblasNoTrans) {
    if (TransA == CblasNoTrans) {
      // C = alpha*A*B+beta*C
      for (int j = 0; j < N; ++j) {
        if (*beta == 0) {
          for (int i = 0; i < M; ++i) {
            C[i+j*ldc] = 0;
          }
        } else if (*beta != 1) {
          for (int i = 0; i < M; ++i) {
            C[i+j*ldc] *= *beta;
          }
        }

        for (int l = 0; l < K; ++l) {
          if (B[l+j*ldb] != 0) {
            temp = *alpha * B[l+j*ldb];
            for (int i = 0; i < M; ++i) {
              C[i+j*ldc] += A[i+l*lda] * temp;
            }
          }
        }
      }

    } else {

      // C = alpha*A**DType*B + beta*C
      for (int j = 0; j < N; ++j) {
        for (int i = 0; i < M; ++i) {
          temp = 0;
          for (int l = 0; l < K; ++l) {
            temp += A[l+i*lda] * B[l+j*ldb];
          }

          if (*beta == 0) {
            C[i+j*ldc] = *alpha*temp;
          } else {
            C[i+j*ldc] = *alpha*temp + *beta*C[i+j*ldc];
          }
        }
      }

    }

  } else if (TransA == CblasNoTrans) {

    // C = alpha*A*B**T + beta*C
    for (int j = 0; j < N; ++j) {
      if (*beta == 0) {
        for (int i = 0; i < M; ++i) {
          C[i+j*ldc] = 0;
        }
      } else if (*beta != 1) {
        for (int i = 0; i < M; ++i) {
          C[i+j*ldc] *= *beta;
        }
      }

      for (int l = 0; l < K; ++l) {
        if (B[j+l*ldb] != 0) {
          temp = *alpha * B[j+l*ldb];
          for (int i = 0; i < M; ++i) {
            C[i+j*ldc] += A[i+l*lda] * temp;
          }
        }
      }

    }

  } else {

    // C = alpha*A**DType*B**T + beta*C
    for (int j = 0; j < N; ++j) {
      for (int i = 0; i < M; ++i) {
        temp = 0;
        for (int l = 0; l < K; ++l) {
          temp += A[l+i*lda] * B[j+l*ldb];
        }

        if (*beta == 0) {
          C[i+j*ldc] = *alpha*temp;
        } else {
          C[i+j*ldc] = *alpha*temp + *beta*C[i+j*ldc];
        }
      }
    }

  }

  return;
}

template <typename DType>
inline void gemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
                 const DType* alpha, const DType* A, const int lda, const DType* B, const int ldb, const DType* beta, DType* C, const int ldc)
{
  if (Order == CblasRowMajor) {
    if (TransA == CblasNoTrans) {
      if (lda < std::max(K,1)) {
        rb_raise(rb_eArgError, "lda must be >= MAX(K,1): lda=%d K=%d", lda, K);
      }
    } else {
      if (lda < std::max(M,1)) { // && TransA == CblasTrans
        rb_raise(rb_eArgError, "lda must be >= MAX(M,1): lda=%d M=%d", lda, M);
      }
    }

    if (TransB == CblasNoTrans) {
      if (ldb < std::max(N,1)) {
        rb_raise(rb_eArgError, "ldb must be >= MAX(N,1): ldb=%d N=%d", ldb, N);
      }
    } else {
      if (ldb < std::max(K,1)) {
        rb_raise(rb_eArgError, "ldb must be >= MAX(K,1): ldb=%d K=%d", ldb, K);
      }
    }

    if (ldc < std::max(N,1)) {
      rb_raise(rb_eArgError, "ldc must be >= MAX(N,1): ldc=%d N=%d", ldc, N);
    }
  } else { // CblasColMajor
    if (TransA == CblasNoTrans) {
      if (lda < std::max(M,1)) {
        rb_raise(rb_eArgError, "lda must be >= MAX(M,1): lda=%d M=%d", lda, M);
      }
    } else {
      if (lda < std::max(K,1)) { // && TransA == CblasTrans
        rb_raise(rb_eArgError, "lda must be >= MAX(K,1): lda=%d K=%d", lda, K);
      }
    }

    if (TransB == CblasNoTrans) {
      if (ldb < std::max(K,1)) {
        rb_raise(rb_eArgError, "ldb must be >= MAX(K,1): ldb=%d N=%d", ldb, K);
      }
    } else {
      if (ldb < std::max(N,1)) { // NOTE: This error message is actually wrong in the ATLAS source currently. Or are we wrong?
        rb_raise(rb_eArgError, "ldb must be >= MAX(N,1): ldb=%d N=%d", ldb, N);
      }
    }

    if (ldc < std::max(M,1)) {
      rb_raise(rb_eArgError, "ldc must be >= MAX(M,1): ldc=%d N=%d", ldc, M);
    }
  }

  /*
   * Call SYRK when that's what the user is actually asking for; just handle beta=0, because beta=X requires
   * we copy C and then subtract to preserve asymmetry.
   */

  if (A == B && M == N && TransA != TransB && lda == ldb && beta == 0) {
    rb_raise(rb_eNotImpError, "syrk and syreflect not implemented");
    /*syrk<DType>(CblasUpper, (Order == CblasColMajor) ? TransA : TransB, N, K, alpha, A, lda, beta, C, ldc);
    syreflect(CblasUpper, N, C, ldc);
    */
  }

  if (Order == CblasRowMajor)    gemm_nothrow<DType>(TransB, TransA, N, M, K, alpha, B, ldb, A, lda, beta, C, ldc);
  else                           gemm_nothrow<DType>(TransA, TransB, M, N, K, alpha, A, lda, B, ldb, beta, C, ldc);

}

#if (defined(HAVE_CBLAS_H) || defined(HAVE_ATLAS_CBLAS_H))
template <>
inline void gemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
          const float* alpha, const float* A, const int lda, const float* B, const int ldb, const float* beta, float* C, const int ldc) {
  cblas_sgemm(Order, TransA, TransB, M, N, K, *alpha, A, lda, B, ldb, *beta, C, ldc);
}

template <>
inline void gemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
          const double* alpha, const double* A, const int lda, const double* B, const int ldb, const double* beta, double* C, const int ldc) {
  cblas_dgemm(Order, TransA, TransB, M, N, K, *alpha, A, lda, B, ldb, *beta, C, ldc);
}

template <>
inline void gemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
          const Complex64* alpha, const Complex64* A, const int lda, const Complex64* B, const int ldb, const Complex64* beta, Complex64* C, const int ldc) {
  cblas_cgemm(Order, TransA, TransB, M, N, K, alpha, A, lda, B, ldb, beta, C, ldc);
}

template <>
inline void gemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
          const Complex128* alpha, const Complex128* A, const int lda, const Complex128* B, const int ldb, const Complex128* beta, Complex128* C, const int ldc) {
  cblas_zgemm(Order, TransA, TransB, M, N, K, alpha, A, lda, B, ldb, beta, C, ldc);
}
#endif

}} // end of namespace nm::math

#endif // GEMM_H


#include <stdio.h>
#include <string.h>
#include <image_lib.h>
#include <gsl/gsl_fit.h>
#include "tz_image_lib.h"
#include "tz_string.h"
#include "tz_fimage_lib.h"
#include "tz_error.h"
#include "tz_darray.h"

#include "private/alignstack.c"
#include "private/alignconf.c"

int main(int argc, const char *argv[])
{
  const char *result_dir = "/Users/zhaot/Work/V3D/neurolabi/data/align";
  const char *image_dir = "/Users/zhaot/Data/nathan/2008-04-18";

  char file_path[150];
  char image_path[150];
  char result_path[150];
  char file1[100], file2[100];
  char id[100];

  fullpath(result_dir, "align_input.txt", file_path);

  FILE *fp = fopen(file_path, "r");

  while ((Read_Word(fp, file1, 0) > 0) &&
	 (Read_Word(fp, file2, 0) > 0)) {
    align_id(file1, file2, id);
    strcat(id, ".txt");
    fullpath(result_dir, id, result_path);
    if (!fexist(result_path)) {
      printf("%s, %s\n", file1, file2);

      fullpath(image_dir, file1, image_path);
      Stack *stack1 = Read_Stack(image_path);
      fullpath(image_dir, file2, image_path);
      Stack *stack2 = Read_Stack(image_path);

      // Stack_Threshold_Tp4(stack1, 0, 65535);
      //Stack_Threshold_Tp4(stack2, 0, 65535);
      int chopoff1 = estimate_chopoff(stack1);
      printf("%d\n", chopoff1);
      Stack *substack1 = Crop_Stack(stack1, 0, 0, chopoff1,
				    stack1->width, stack1->height,
				    stack1->depth - chopoff1, NULL);
      int chopoff2 = estimate_chopoff(stack2);
      printf("%d\n", chopoff2);
      Stack *substack2 = Crop_Stack(stack2, 0, 0, chopoff2,
				    stack2->width, stack2->height,
				    stack2->depth - chopoff2, NULL);

      float unnorm_maxcorr;
      int offset[3];

      int intv[] = {3, 3, 3};
      float score = Align_Stack_MR_F(substack1, substack2, intv, 1, offset,
				     &unnorm_maxcorr);

      if (score <= 0.0) {
	printf("failed\n");
	write_align_result(result_dir, file1, file2, NULL);
      } else {
	offset[2] += chopoff2;
	printf("(%d, %d, %d): %g\n", offset[0], offset[1], offset[2], score);
	write_align_result(result_dir, file1, file2, offset);
      }

      Kill_Stack(substack1);
      Kill_Stack(substack2);
      Kill_Stack(stack1);
      Kill_Stack(stack2);
    }
  }

  fclose(fp);

  return 0;
}


/*
Copyright (C) 2019-2020 JingWeiZhangHuai <jingweizhanghuai@163.com>
Licensed under the Apache License, Version 2.0; you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
*/
#include "morn_util.h"

#include <cblas.h>
#ifdef MORN_USE_CL
#include <CL/cl.h>
#include <clBLAS.h>
cl_context mDeviceContext(int device);
cl_command_queue mDeviceQueue(int device);
#endif

#define MORN_NO_TRANS 0
#define MORN_TRANS    1

struct HandleCLBlas
{
    int clblas_setup;
};
void endCLBlas(struct HandleCLBlas *handle)
{
    NULL;
    #ifdef MORN_USE_CL
    if(handle->clblas_setup) clblasTeardown();
    #endif
}
#define HASH_CLBlas 0x3b4b7c3a
static struct HandleCLBlas *morn_clblas_handle=NULL;
void SetupCLBlas()
{
    MHandle *hdl=mHandle(mMornObject(NULL,DFLT),CLBlas);
    morn_clblas_handle=(struct HandleCLBlas *)(hdl->handle);
    if(hdl->valid==0)
    {
        #ifdef MORN_USE_CL
        mException(clblasSetup()!=CL_SUCCESS,EXIT,"CLBlas not work");
        morn_clblas_handle->clblas_setup=1;
        #endif
        hdl->valid=1;
    }
}

void mSgemm(int device,int a_trans,int b_trans,int m,int n,int k,float alpha,MMemoryBlock *a,int sa,MMemoryBlock *b,int sb,float beta,MMemoryBlock *c,int sc)
{
    #ifdef MORN_USE_CL
    if(device!=MORN_HOST)
    {
        a_trans=(a_trans==MORN_TRANS)?clblasTrans:clblasNoTrans;
        b_trans=(b_trans==MORN_TRANS)?clblasTrans:clblasNoTrans;

        if(morn_clblas_handle==NULL) SetupCLBlas();

        device = c->device;
        mMemoryBlockCopy(a,device);
        mMemoryBlockCopy(b,device);
        cl_event a_event=(cl_event)(a->cl_evt);
        cl_event b_event=(cl_event)(b->cl_evt);
        cl_event c_event=(cl_event)(c->cl_evt);

        cl_event event_list[2] = {a_event,b_event};
        cl_command_queue queue=mDeviceQueue(device);
        int ret=clblasSgemm(clblasRowMajor,a_trans,b_trans,m,n,k,alpha,a->cl_data,0,sa,b->cl_data,0,sb,beta,c->cl_data,0,sc,1,&queue,2,event_list,&c_event);
        mException(ret!=CL_SUCCESS,EXIT,"clblas error");

        c->flag = MORN_DEVICE;
        return;
    }
    #endif
    a_trans=(a_trans==MORN_TRANS)?CblasTrans:CblasNoTrans;
    b_trans=(b_trans==MORN_TRANS)?CblasTrans:CblasNoTrans;
    cblas_sgemm(CblasRowMajor,a_trans,b_trans,m,n,k,alpha,a->data,sa,b->data,sb,beta,c->data,sc);
}

char *saxpby=mString(
__kernel void saxpby(__global const float* a,__global float* b,const float alpha,const float beta,const int sa,const int sb)
{
    const int idx = get_global_id(0);
    const int ia = idx*sa;
    const int ib = idx*sb;
    b[ib]=a[ia]*alpha+b[ib]*beta;
});

void mSaxpby(int device,int n,float alpha,MMemoryBlock *a,int sa,float beta,MMemoryBlock *b,int sb)
{
    #ifdef MORN_USE_CL
    if(device!=MORN_HOST)
    {
        mCLFunction(saxpby,CLSIZE(n),CLIN(a),CLINOUT(b),CLPARA(&alpha,sizeof(float)),CLPARA(&beta,sizeof(float)),CLPARA(&sa,sizeof(int)),CLPARA(&sb,sizeof(int)));
        return;
    }
    #endif
    cblas_saxpby(n,alpha,a->data,sa,beta,b->data,sb);
}


//
// cngsl.h
//
// Copy Number Utilities that use gsl
//
// copyright 2017 Peter Andrews
//

#ifndef PAA_CNGSL_H_
#define PAA_CNGSL_H_

#include <gsl/gsl_cdf.h>

#include <algorithm>
#include <limits>
#include <string>
#include <vector>

#include "cn.h"

namespace paa {

class CNStateCall {
 public:
  CNStateCall(const std::vector<double> & state_probs_,
              const unsigned int expected_state_,
              const double segment_count_) :
      state_probs{state_probs_},
    not_state_probs(state_probs.size()),
    expected_state{expected_state_},
    segment_count{segment_count_} {
    double loss_prob{0.0};
    double not_loss_prob{0.0};
    double expected_prob{0.0};
    double not_expected_prob{0.0};
    double gain_prob{0.0};
    double not_gain_prob{0.0};
    for (unsigned int s{0}; s != state_probs.size(); ++s) {
      if (state_probs[s] > state_probs[best_call]) {
        best_call = s;
      }
      if (s < expected_state) {
        loss_prob += state_probs[s];
        not_gain_prob += state_probs[s];
        not_expected_prob += state_probs[s];
      } else if (s > expected_state) {
        gain_prob += state_probs[s];
        not_loss_prob += state_probs[s];
        not_expected_prob += state_probs[s];
      } else {
        expected_prob += state_probs[s];
        not_loss_prob += state_probs[s];
        not_gain_prob += state_probs[s];
      }
    }
    for (unsigned int n{0}; n != state_probs.size(); ++n) {
      for (unsigned int s{0}; s != state_probs.size(); ++s) {
        if (n == s) continue;
        not_state_probs[n] += state_probs[s];
      }
    }

    type = best_call < expected_state ? "loss" :
        (best_call > expected_state ? "gain" : "norm");
    const double min_prob{std::numeric_limits<double>::min()};
    score = -log10(max(best_call < expected_state ? not_loss_prob :
                       (best_call > expected_state ? not_gain_prob :
                        not_expected_prob),
                       min_prob));
    prob_not_expected = max(not_expected_prob, min_prob);
    prob_not_loss = max(not_loss_prob, min_prob);
    prob_not_gain = max(not_gain_prob, min_prob);
    }

  std::vector<double> state_probs;
  std::vector<double> not_state_probs;
  unsigned int expected_state;
  unsigned int best_call{0};
  double segment_count{0.0};
  std::string type{""};
  double score{0.0};
  double prob_not_expected{0.0};
  double prob_not_loss{0.0};
  double prob_not_gain{0.0};
};

class CNStateCaller {
 public:
  CNStateCaller(const Reference & ref_,
                const std::vector<Bin> & bins_,
                const CN_Bins & profile_) :
      ref{ref_}, bins{bins_}, profile{profile_},
    x_chr{ref.find_x_chromosome()},
    y_chr{ref.find_y_chromosome()},
    is_male{is_profile_male()},
    expected_states{get_expected_states()},
    average_autosome_count{get_average_autosome_count()},
    zero_state_count{average_autosome_count / 10} { }

  double get_average_autosome_count() const {
    double count{0};
    unsigned int n_count{0};
    for (unsigned int b{0}; b != bins.size(); ++b) {
      const unsigned int chr{bins[b].chromosome()};
      if (chr == x_chr || chr == y_chr) continue;
      count += profile[b].norm_count();
      ++n_count;
    }
    return count / n_count;
  }

  std::vector<unsigned char> get_expected_states() const {
    std::vector<unsigned char> result;
    for (unsigned int c{0}; c != ref.n_chromosomes(); ++c) {
      const bool is_x{c == x_chr};
      const bool is_y{c == y_chr};
      if (is_male && (is_x || is_y)) {
        result.push_back(1);
      } else if (!is_male && is_y) {
        result.push_back(0);
      } else {
        result.push_back(2);
      }
    }
    return result;
  }

  bool is_profile_male() const {
    double ratio{0};
    unsigned int n_ratio{0};
    for (unsigned int b{0}; b != bins.size(); ++b) {
      const unsigned int chr{bins[b].chromosome()};
      if (chr == y_chr) {
        ratio += profile[b].ratio();
        ++n_ratio;
      }
    }
    return ratio / n_ratio > 0.3333;
  }

  CNStateCall call(const unsigned int start,
                   const unsigned int stop) const {
    const double par_fraction{fraction_par(start, stop)};
    const unsigned int expected_par_copy{2};
    const unsigned int expected_state{static_cast<unsigned int>(
        0.5 + par_fraction * expected_par_copy +
        (1 - par_fraction) * expected_states[bins[start].chromosome()])};
    const double segment_count{[start, stop, this]() {
        double result{0};
        for (unsigned int b{start}; b != stop; ++b) {
          result += profile[b].norm_count();
        }
        return result;
      }()};

    std::vector<double> state_chi_squareds(n_states);
    std::vector<double> state_probs(n_states);
    double total_prob{0.0};
    double diff_power{2.0};
    while (total_prob <= 0) {
      for (unsigned int s{0}; s != n_states; ++s) {
        const double expected_count{(stop - start) *
              (s ? s * average_autosome_count / 2 : zero_state_count)};
        const double diff{expected_count > segment_count ?
              expected_count - segment_count : segment_count - expected_count};
        const double chi_squared{pow(diff, diff_power) / expected_count};
        state_chi_squareds[s] = chi_squared;
        const double prob{gsl_cdf_chisq_Q(chi_squared, 1)};
        total_prob += state_probs[s] = prob;
      }
      diff_power -= 0.1;
    }
    diff_power += 0.1;

    for (unsigned int s{0}; s != n_states; ++s) {
      state_probs[s] /= total_prob;
    }

    return CNStateCall{move(state_probs), expected_state, segment_count};
  }

  double fraction_par(const unsigned int start,
                      const unsigned int stop) const {
    if (bins[start].chromosome() != x_chr) {
      return 0.0;
    }
    const unsigned int par_coords[2][2]{{60000, 2699520},
      {154931043, 155260560}};
    if (bins[stop - 1].stop_position() <= par_coords[0][0] ||
        bins[start].start_position() >= par_coords[1][1] ||
        (bins[start].start_position() >= par_coords[0][1] &&
         bins[stop - 1].stop_position() <= par_coords[1][0])) {
      return 0.0;
    }
    double result{0.0};
    for (unsigned int b{start}; b != stop; ++b) {
      double frac_pos{0.0};
      for (const unsigned int p : {0, 1}) {
        const unsigned int par_len{par_coords[p][1] - par_coords[p][0]};
        if (bins[b].start_position() <= par_coords[p][0]) {
          if (bins[b].stop_position() >= par_coords[p][1]) {
            frac_pos += 1.0 * par_len / bins[b].length();
          } else if (bins[b].stop_position() >= par_coords[p][0]) {
            frac_pos += 1.0 * (bins[b].stop_position() - par_coords[p][0]) /
                bins[b].length();
          }
        } else if (bins[b].start_position() <= par_coords[p][1]) {
          if (bins[b].stop_position() <= par_coords[p][1]) {
            frac_pos += 1.0;
          } else {
            frac_pos += 1.0 * (par_coords[p][1] - bins[b].start_position()) /
                bins[b].length();
          }
        }
      }
      result += frac_pos;
    }
    return result / (stop - start);
  }

  const Reference & ref;
  const std::vector<Bin> & bins;
  const CN_Bins & profile;
  const unsigned int x_chr;
  const unsigned int y_chr;
  const bool is_male;
  const std::vector<unsigned char> expected_states;
  const double average_autosome_count;
  const double zero_state_count;
  const unsigned int n_states{20};
};

}  // namespace paa

#endif  // PAA_CNGSL_H_


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

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

#define COMPLEX
void batch_zgemv(
	const enum BBLAS_TRANS *trans,
	const int *m, const int *n,
	const BBLAS_Complex64_t *alpha,
	const BBLAS_Complex64_t **arrayA, const int *lda,
	const BBLAS_Complex64_t **arrayx, const int *incx,
	const BBLAS_Complex64_t *beta,
	BBLAS_Complex64_t **arrayy, const int *incy,
	const int batch_count, const enum BBLAS_OPTS batch_opts,
	int* info)
{
	/* Local variables */
	char func_name[15] = "batch_zgemv";

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

	if (batch_opts == BBLAS_FIXED)
	{
		/* Call fixed size code */
		batchf_zgemv(trans[0],
					 m[0], n[0],
					 alpha[0],
					 arrayA, lda[0],
					 arrayx, incx[0],
					 beta[0],
					 arrayy, incy[0],
					 batch_count, info[0]);
	}
	else if (batch_opts == BBLAS_VARIABLE)
	  {
		/* Call variable size code */
		batchv_zgemv(trans,
					 m, n,
					 alpha,
					 arrayA, lda,
					 arrayx, incx,
					 beta,
					 arrayy, incy,
					 batch_count, info);
	}
	else
	{
		xerbla_batch(func_name, BBLAS_ERR_BATCH_OPTS, -1);
	}
}