bool interval::subset_of(const interval& x) const {

	ASSERT2(lb <= ub, *this);
	ASSERT2(x.lb <= x.ub, x);

	return lb >= x.lb && ub <= x.ub;
}

const interval log(const interval& x) {

	ASSERT2(x.lb <= x.ub, "x: "<<x);
	ASSERT2(0<x.lb, "x.lb = "<<x.lb);

	return interval(std::log(x.lb), std::log(x.ub));
}

int test2(int seed) {
	struct gameState *G = newGame();
	//struct gameState *Gcpy = newGame();
	int i;
	int numPlayers;
	int ret;
	int *k = getUniqueCards();
	printf("TEST #2: Number of players random testing\n");
	printf("\t should return 0 if 2 - 4; -1 otherwise\n");
	//random testing for number of players
	for(i = 0; i < NUMTRIALS; i++) {
		numPlayers = rand();
		ret = initializeGame(numPlayers, k, seed, G);
		if((numPlayers == 1) || (numPlayers == 2) || (numPlayers == 3) || (numPlayers == 4))
		{
			ASSERT2(ret, 0, "FAIL");
		}
		else
		{
			ASSERT2(ret, -1, "FAIL");
		}
	}
	printf("PASS\n");
	printf("----------------------\n");
	free(G);
	free(k);
}

int test1(int seed) 
{
	int i, j;
	int *k = (int*)malloc(sizeof(int)*NUMCARDS);

	int numPlayers = (rand()% 3) + 2;
	int ret = -1;

	struct gameState *G = newGame();
//	struct gameState *Gcpy = newGame();
	//repeat until you get return val 0 in initializeGame, i.e. all kingdom cards are unique

	printf("TEST #1: Kingdom cards random testing\n");
	printf("\t should return 0 if unique; -1 if non-unique\n");
	for(i = 0; i < NUMTRIALS; i++)
	{
		for(j = 0; j < NUMCARDS; j++) {
			k[j] = (rand()%15);
		}
		ret = initializeGame(numPlayers, k, seed, G);
		int cret = cardsAreUnique(k);
		if(cret == 0) {
			ASSERT2(ret, 0, "FAIL when cards are unique");
		}
		else {
			ASSERT2(ret, -1, "FAIL when cards are non-unique");
		}
	}

	printf("PASS\n");
	printf("------------------\n");
	free(G);
	free(k);
}

void perform_transfer(urb_t *urb) {
	int direction = urb->direction == IN ? LIBUSB_ENDPOINT_IN : LIBUSB_ENDPOINT_OUT;

	/* Take a care about timings */
	if(urb->timing) {
		int time = urb->timing*pow(10,6);
		usleep(time);
	}

	/* Trigger libusb to perform the transfer */
	int r, bytes_transferred = 0;
	struct libusb_transfer transfer;
	switch(urb->type) {
		case CTRL:
			r = libusb_control_transfer(dev_handle, 
					urb->bmRequestType | direction,
					urb->bRequest,
					urb->wValue,
					urb->wIndex,
					urb->data,
					urb->data_size,
					0);
			ASSERT2(r >= 0, TRANSFER_FAILED_MESSAGE, libusb_error_name(r));
			break;
		case BULK:
			r = libusb_bulk_transfer(dev_handle,
					urb->endpoint | direction,
					urb->data,
					urb->data_size,
					&bytes_transferred,
					0);
			ASSERT2(r == 0, TRANSFER_FAILED_MESSAGE, libusb_error_name(r));
			break;
		case INTR:
			r = libusb_interrupt_transfer(dev_handle,
					urb->endpoint | direction,
					urb->data,
					urb->data_size,
					&bytes_transferred,
					0);
			ASSERT2(r == 0, TRANSFER_FAILED_MESSAGE, libusb_error_name(r));
			break;
		case ISOC:
			libusb_fill_iso_transfer(&transfer,
					dev_handle,
					urb->endpoint | direction,
					urb->data,
					urb->data_size,
					1,
					continue_transfer_cb,
					NULL,
					0);
			libusb_submit_transfer(&transfer);
			ASSERT2(r == 0, TRANSFER_FAILED_MESSAGE, libusb_error_name(r));
			break;
	}
}

void index_set::collect_type2_common_subexpressions() {

	ASSERT2(current.empty(),"recording not finished or not run");
	ASSERT2(type2_cse.empty(),"this function has already been called");

	for (int i=0; i<number_of_constraints(); ++i) {

		check_for_common_subexpressions(i);
	}
}

static const char *skip_to_params(const char *input, const struct CmdTemplate *cmdp)
{
	const char *begin = input, *end = input;

	// Skip the ID, and get the command
	if (!get_word(&begin, &end))
		return NULL;
	if (!get_word(&begin, &end))
		return NULL;

	ASSERT2(strlen(cmdp->name) == (size_t)(end-begin), "Invalid command template specified");
	ASSERT2(!strncmp(begin, cmdp->name, end-begin), "Invalid command template specified");

	return end;
}

double port_impl::col_val(int i) const {

	ASSERT(i<=M); // X.size() == N

	double val = X.at(i-1);

	const double lb = col_lb(i);

	const double ub = col_ub(i);

	ASSERT2(lb<ub,"lb, ub: "<<lb<<", "<<ub);

	if ((val+1.0e-4 < lb) || (val > ub+1.0e-4)) {

		ASSERT(false);
		//throw numerical_problems();
	}

	if (val < lb) {

		val = lb;
	}
	else if (val > ub) {

		val = ub;
	}

	return val;
}

void FreeGaussMixModel(GaussMixModel *p_gmmParam,int p_nModelNum)
{
	if (p_gmmParam)
	{
		ASSERT2(p_nModelNum,"Error call FreeGaussMixModel() : p_nModelNum<0!");
		for (int i=0;i<p_nModelNum;i++)
		{
			if (p_gmmParam[i].pGauss)		
			{
				Free(p_gmmParam[i].pfMeanBuf);
				Free(p_gmmParam[i].pfDiagCovBuf);

				Free(p_gmmParam[i].pGauss);

				p_gmmParam[i].pfMeanBuf = NULL;
				p_gmmParam[i].pfDiagCovBuf = NULL;
				p_gmmParam[i].pGauss = NULL;
			}
			Free(p_gmmParam[i].pfWeight);
			p_gmmParam[i].pfWeight = NULL;
		}
		Free(p_gmmParam);
		p_gmmParam = NULL;
	}
}

/**
 * \brief Command arguments parser.
 *
 * Using the format pointed by the argument fmt
 * parses the input string filling the array argv
 * with input parameters of the correct type.
 *
 * \param fmt   Parameters format string.
 * \param input Input string.
 * \param argv  Array filled with parameters.
 *
 * \return False in case of errors, otherwise true.
 */
static bool parseArgs(const char *fmt, const char *input, parms argv[])
{
	const char *begin = input, *end = input;

	while (*fmt)
	{
		// Extract the argument
		if (!get_word(&begin, &end))
			return false;

		switch (*fmt)
		{
			case 'd':
				(*argv++).l = atol(begin);
				break;

			case 's':
				(*argv++).s = begin;
				break;

			default:
				ASSERT2(0, "Unknown format for argument");
				return false;
		}

		++fmt;
	}

	/* check if there are remaining args */
	if (get_word(&begin, &end))
		return false;

	return true;
}

const DoubleArray2D Bratu<T>::solutions() const {

	ASSERT2(n_vars==SIZE,"n_vars: "<<n_vars)

	ASSERT2(SOLS==n_sol,"n_sol: "<<n_sol);

	DoubleArray2D solution_vectors(SOLS);

	for (int i=0; i<SOLS; ++i) {

		const double* const x = sol[i];

		solution_vectors.at(i).assign(x, x + SIZE);
	}

	return solution_vectors;
}

void load_id(FILE *f, const string &s) {
    string s2;
    load(f, s2);
    if (s2 != s) {
        fprintf(stderr, "Error: load_id expected |%s|, received |%s|\n", s.c_str(), s2.c_str());
        ASSERT2(false, "load from file failed: load_id expected matching string");
    }
}

uint8_t combuf_read(const combuf_t combuf, const combuf_pos_t pos)
{
    ASSERT1(CHECK_COMBUF(combuf), "invalid combuf = %d", combuf);
    ASSERT2(pos < PAYLOAD_SIZE(combuf),
            "invalid pos = %hhu ( combuf = %d)", pos, combuf);

    return PAYLOAD_ITEM(combuf, pos);
}

int port_impl::find_index_position(int i) const {

	std::vector<int>::const_iterator itr = std::find(ISIMP.begin(), ISIMP.end(), i);

	ASSERT2(itr!=ISIMP.end(),"index not found: "<<i);

	return itr - ISIMP.begin();
}

int test3c(int seed) {
	int ret;
	int numPlayers;
	int curseCount, estateCount, duchyCount, provinceCount, copperCount, silverCount = 40, goldCount = 30;
	struct gameState *G = newGame();
	int *k = getUniqueCards();

	//Test when players are 4
	numPlayers = 4;
	ret = initializeGame(numPlayers, k, seed, G);
	curseCount = G->supplyCount[curse];
	estateCount = G->supplyCount[estate];
	duchyCount = G->supplyCount[duchy];
	provinceCount = G->supplyCount[province];
	copperCount = G->supplyCount[copper];
	silverCount = G->supplyCount[silver];
	goldCount = G->supplyCount[gold];
	ASSERT2(curseCount, 30, "curseCount");
	ASSERT2(estateCount, 12, "estateCount");
	ASSERT2(duchyCount, 12, "duchyCount");
	ASSERT2(provinceCount, 12, "provinceCount");
	ASSERT2(copperCount, (60 - (7 * numPlayers)), "copperCount");
	ASSERT2(silverCount, 40, "silverCount");
	ASSERT2(goldCount, 30, "goldCount");
	free(G);
	free(k);
}

const interval sqr(const interval& x) {

	ASSERT2(x.lb <= x.ub, "x: "<<x);

	double lb(std::pow(x.lb, 2)), ub(std::pow(x.ub, 2));

	swap_if_necessary(lb, ub);

	return (x.lb<=0 && 0<=x.ub) ? interval(0, ub) : interval(lb, ub);
}

//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
                      btRigidBody& body2, const btVector3& pos2,
                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
{
	(void)timeStep;
	(void)distance;


	btScalar normalLenSqr = normal.length2();
	ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
	if (normalLenSqr > btScalar(1.1))
	{
		impulse = btScalar(0.);
		return;
	}
	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
	//this jacobian entry could be re-used for all iterations
	
	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
	btVector3 vel = vel1 - vel2;
	

	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
		body2.getCenterOfMassTransform().getBasis().transpose(),
		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
		body2.getInvInertiaDiagLocal(),body2.getInvMass());

	btScalar jacDiagAB = jac.getDiagonal();
	btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
	
	  btScalar rel_vel = jac.getRelativeVelocity(
		body1.getLinearVelocity(),
		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
		body2.getLinearVelocity(),
		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
	btScalar a;
	a=jacDiagABInv;


	rel_vel = normal.dot(vel);
	
	//todo: move this into proper structure
	btScalar contactDamping = btScalar(0.2);

#ifdef ONLY_USE_LINEAR_MASS
	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
	impulse = - contactDamping * rel_vel * massTerm;
#else	
	btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
	impulse = velocityImpulse;
#endif
}

void AllocGaussMixModel(GaussMixModel *p_pModel,int p_nMixNum,int p_nVecSize)
{
	ASSERT2(p_nMixNum,"Error call AllocGaussMixModel() : p_nMixNum<0!");
	ASSERT2(p_nVecSize,"Error call AllocGaussMixModel() : p_nVecSize<0!");
	
	// 分配
	p_pModel->pfWeight = (float *)Malloc(p_nMixNum*sizeof(float));
	p_pModel->pGauss = (GaussPDF*)Malloc (p_nMixNum*sizeof(GaussPDF));

	p_pModel->pfMeanBuf = (float *)Malloc(p_nMixNum*sizeof(float)*p_nVecSize,true);
	p_pModel->pfDiagCovBuf = (float *)Malloc(p_nMixNum*sizeof(float)*p_nVecSize,true);
	for(int m=0;m<p_nMixNum;m++)
	{
		p_pModel->pGauss[m].pfMean = p_pModel->pfMeanBuf + m*p_nVecSize;
		p_pModel->pGauss[m].pfDiagCov = p_pModel->pfDiagCovBuf + m*p_nVecSize;
	}

	// 初始化
	p_pModel->pfWeight[0] = 1.f;
}

result_t combuf_write(const combuf_t combuf,
                      const combuf_pos_t pos, const uint8_t data)
{
    ASSERT1(CHECK_COMBUF(combuf), "invalid combuf = %d", combuf);
    ASSERT2(pos < PAYLOAD_SIZE(combuf),
            "invalid pos = %hhu ( combuf = %d)", pos, combuf);

    PAYLOAD_ITEM(combuf, pos) = data;

    return ENOERR;
}