//--------------------------------------------------------------
void testApp::draw(){
    centroid = center;
    cam.setPosition(ofVec3f(0, 0, -centroid.z));
    
    cam.lookAt(centroid, ofVec3f(0,1,0));
    cam.setFarClip(50000);
    
    cam.begin();
    
    ofPushMatrix();
    ofTranslate(camPosX, camPosY, camZoom);
    
    if(bTop){
        pivot(centroid, 100, 0, 0);
    }
    else{
        pivot(centroid, camRotX, camRotY, 0);
    }
    
    ofScale(-1.0, 1.0, 1.0);
    drawAxes(centroid, refPoint);
    
    ofPushStyle();
    
    //-------------------------
    for(int i = 0; i < K; i++)
        kinects[i].draw();
    
    
    if(bCalibrated && bTracking){
        for(int i = 0; i < N; i++)
            trackers[i].draw();
        
        for(int i = 0; i < N - 1; i++)
            for(int j = 1; j < N; j++){
                setLineColor(i + j);
                ofLine(trackers[i].lerpedPos, trackers[j].lerpedPos);
            }
        
        //-------------------------
        ofEnableAlphaBlending();
        ofSetColor(255, 0, 0, 50);
        ofFill();
        ofBeginShape();
        for(int i = 0; i < N; i++)
            ofVertex(trackers[i].lerpedPos);
        ofEndShape();
        ofDisableAlphaBlending();
        //-------------------------
    }
    
    ofPopStyle();
   

    ofPopMatrix();

    cam.end();
    
    gui.draw();    
}

bool  Forward_DepthF_Search(vind frwind0,vind fvind,vind lvind,vind nvfrwd)  
{
	vind nv,minnv,maxnv,minnvfrd,maxnvfrd;
	vind t,frwind(frwind0);
	real maxstcrt(NOBND);
	subsetdata* prvdatapt;

	if (lvind-fvind > 10) {
		newtime = clock();
		if (newtime==clock_t(-1))  {
			msg("Eleaps error: time overflow\n");
			return false;
		}
		rtime -= static_cast<double>(newtime-ctime);
		if (rtime < 0.) return false;  // Exit if time limit was exceded
		ctime = newtime; 
	} 

//  Find maximal subset dimensionalities of current tree branches
	
	if ( (maxnvfrd=nvfrwd+lvind-fvind+1) > maxdim) maxnvfrd = maxdim;

//  Start pivoting variables 

	{ for (vind u=fvind;u<=lvind;u++)  {

		t = lvind-u; 

//		Set number of variables in the susbset where the current pivot will be performed
		nv = minnvfrd = nvfrwd+u-fvind+1;

		if (maxnvfrd >= mindim && minnvfrd <= maxdim)  {

//			Make a pivot	
			if (minnvfrd < mindim) pivot(SW,SRC,frwind,t,nv,u,t,mindim,maxnvfrd,false);
			else if (minnvfrd < maxdim) pivot(SW,SRC,frwind,t,nv,u,t,minnvfrd,maxnvfrd,false);
				 else pivot(SW,SRC,frwind,0,nv,u,t,minnvfrd,maxnvfrd,false);
		}
		if (t > 0) {	//	Keep track of source memory for current forward search results
			prvks[t-1] = frwind; 
			frwind = t;	// Update memory index 
		}
	} }

//  Process recursevly the subtrees created by the previous cycle

	{ for (vind i=0;i<lvind-fvind;i++)   {
		minnv = nvfrwd+lvind-fvind-i;
		maxnv = nvfrwd+lvind-fvind;

		if (minnv <= maxdim && maxnv >= mindim) 
	 		if (!Forward_DepthF_Search(prvks[i],lvind-i,lvind,minnv-1)) return false;
	} }

	return true;
}

void wrkspace::initwrkspace(bool pivotall,vind nv,subsetdata* data0,vind lstind,vind nvattop,vind nvatbot,
				vector<vind>& vattop,vector<vind>& vatbot)
{
	vind lastv;
	vector<vind> tlst; 
	subsetdata* newdata=0;
	double trivbnd(-INF);

	p = nv;
	maxim = data0->max();
	if ( !max() ) trivbnd *= -1;
	wrklst.assign(lstind+1,static_cast<subset *>(0));
	if (pivotall)  {
		lastv = nv;
		nwl = p-fp-lp+1;
	}
	else  {
		lastv = nv-1;
		nwl = p-fp-lp;
	}
	if (fp+lp > 0)  {
		tlst.resize(p);
		frontlsts(vatbot,vattop,nvatbot,nvattop,tlst);
		(wrklst[lstind] = new subset(tlst,p,p,data0,false,p))->reorder(tlst);
	}
	else wrklst[lstind] = new subset(p,p,data0,false,p);

	for (vind j=1;j<=nvattop;j++)  {
		newdata = data0->crcopy(p,p-nvatbot-j);
		try {
			if (fp+lp==0) wrklst[lstind-j] = new subset(p,p-nvatbot-j,newdata,true,p);
			else wrklst[lstind-j] = new subset(tlst,p,p-nvatbot-j,newdata,true,p);
		}
		catch (...)   {
			delete newdata;
			throw;
		}
		if (lstind > j) pivot(nvatbot+j,p-nvatbot-j,lstind+1-j,lstind-j,trivbnd);
		else pivot(nvatbot+j,0,lstind+1-j,0,trivbnd);
		delete wrklst[lstind+1-j];
	}

	for (vind j1=nwl-2;j1>=0;j1--) {
		newdata = data0->crcopy(lastv,j1); 
		try {
			if (fp+lp==0) wrklst[j1] = new subset(lastv,j1,newdata,true,p);
			else wrklst[j1] = new subset(tlst,lastv,j1,newdata,true,p);
		}
		catch (...)   {
			delete newdata;
			throw; 		

		}
	}
}

    int solve(long long& res){ /// simplex core
        int i, j, x, y;
        long long u, v, mn, mx;
        for (i = 1; i <= n; i++) down[i] = i;
        for (i = 1; i <= m; i++) link[i] = i + n;

        while (1){ /// phase 1
            x = 0, y = 0, mn = -EPS;
            for (i = 1; i <= m; i++){
                if (rhs[i] < mn) mn = rhs[i], x = i;
            }
            if (x == 0) break;

            for (i = 1; i <= n; i++){
                if (ar[x][i] < -EPS){
                    y = i;
                    if (rand() & 1) break;
                }
            }
            if (y == 0) return INFEASIBLE;
            pivot(x, y, res);
        }

        while (1){ /// phase 2
            x = 0, y = 0, mx = EPS;
            for (i = 1; i <= n; i++){
                if (ar[0][i] > mx) mx = ar[0][i], y = i;
            }
            if (y == 0) break;

            for (i = 1; i <= m; i++){
                if (ar[i][y] > EPS){
                    u = rhs[i] / ar[i][y];
                    if (x == 0 || u < v) x = i, v = u;
                }
            }
            if (x == 0) return UNBOUNDED;
            pivot(x, y, res);
        }

        res *= flag;
        for (int i = 1; i <= m; i++){
            if(link[i] <= n) idx[link[i]] = i;
        }

        for (int i = 1; i <= n; i++) val[i] = rhs[idx[i]];
		return FEASIBLE;
	}

static int minimize() {
  int i, ipivot, jpivot;
  mpfr_t t, u;
  mpfr_inits(t, u, (mpfr_ptr)0);

  for (;;) {
    for (jpivot = 1; jpivot <= jmax; jpivot++) {
      if (row[jpivot] == 0) {
	tableau(pivotcolumn[0], 0, jpivot);
	if (mpfr_cmp(pivotcolumn[0], minuseps) < 0) break;
      }
    }
    if (jpivot > jmax) {
      mpfr_clears(t, u, (mpfr_ptr)0);
      return 1;
    }

    mpfr_set(u, large, GMP_RNDN);
    ipivot = 0;
    for (i = 1; i <= m; i++) {
      tableau(pivotcolumn[i], i, jpivot);
      if (mpfr_cmp(pivotcolumn[i], eps) > 0) {
	tableau(t, i, 0);
	mpfr_div(t, t, pivotcolumn[i], GMP_RNDN);
	if (mpfr_cmp(t, u) < 0) { ipivot = i; mpfr_set(u, t, GMP_RNDN); }
      }
    }
    if (ipivot == 0) {
      mpfr_clears(t, u, (mpfr_ptr)0);
      return 0; // the objective function can be minimized to -infinite
    }
    pivot(ipivot, jpivot);
  }
}

void main(int argc, char **argv) {
  uid_t uid = getuid();
  uid_t gid = getgid();
  char *sandbox_root;
  char **cmd_args;

  if(argc < 3) {
    fprintf(stderr, "Usage: sandbox_root command...\n");
    exit(-1);
  }

  new_root = argv[1];
  cmd_args = argv+2;

  /* 1. Unshare. From here onwards, we have most root-equiv capabilities until we execve-ed, FIXME LWN article here. */
  // FIXME try clone here, just to see if that works too.
  check("unshare", unshare(CLONE_NEWNS | CLONE_NEWUSER | CLONE_NEWPID | CLONE_NEWUTS));
  /* 2. Fork once, so we become the parent of the new process id space and can mount proc */
  fork_once();

  /* 3. Set UID/GID maps, FIXME LWN article here */
  write_file("/proc/self/setgroups", "deny");
  write_file("/proc/self/uid_map", aasprintf("%d %d 1", uid, uid));
  write_file("/proc/self/gid_map", aasprintf("%d %d 1", gid, gid));

  /* 4. Mount minimal fstab */
  mount_fstab();

  /* 5. Pivot into the new root */
  pivot();

  /* 6. Exec */
  check("execve", execv(cmd_args[0], cmd_args));
}

void qs(int a[], int l, int r)
{
    if(l == r || l>r)
        return;

    if((r-l) == 1)
    {
        if(a[l] > a[r])
            swap(&a[l],&a[r]);
        return;
    }

    int index = pivot(a,l,r,2);
    swap(&a[index],&a[l]);
    int lt = l+1, rt = r;
    while(lt < rt)
    {
        while(a[lt] < a[l])
            lt++;
        while(a[rt] > a[l])
            rt--;
        if(lt < rt)
        {
            swap(&a[lt],&a[rt]);
            lt++;
            rt--;
        }

    }
    if(l != rt)
        swap(&a[l],&a[rt]);
    qs(a,l,rt-1);
    qs(a,rt+1,r);
    return;
}

//static
uint KThumb::imageVariance(QImage image )
{
    uint delta = 0;
    uint avg = 0;
    uint bytes = image.numBytes();
    uint STEPS = bytes/2;
    QVarLengthArray<uchar> pivot(STEPS);
    const uchar *bits=image.bits();
    // First pass: get pivots and taking average
    for( uint i=0; i<STEPS ; i++ ){
        pivot[i] = bits[2 * i];
#if QT_VERSION >= 0x040700
        avg+=pivot.at(i);
#else
        avg+=pivot[i];
#endif
    }
    avg=avg/STEPS;
    // Second Step: calculate delta (average?)
    for (uint i=0; i<STEPS; i++)
    {
#if QT_VERSION >= 0x040700
        int curdelta=abs(int(avg - pivot.at(i)));
#else
        int curdelta=abs(int(avg - pivot[i]));
#endif
        delta+=curdelta;
    }
    return delta/STEPS;
}

rspfImageViewAffineTransform::rspfImageViewAffineTransform(double rotateDegrees,
                                                             double scaleXValue,
                                                             double scaleYValue,
                                                             double translateXValue,
                                                             double translateYValue,
                                                             double pivotXValue,
                                                             double pivotYValue)
   :m_transform(3,3),
    m_inverseTransform(3,3),
    m_rotation(rotateDegrees),
    m_scale(scaleXValue, scaleYValue),
    m_translate(translateXValue, translateYValue),
    m_pivot(pivotXValue, pivotYValue)
{
   m_transform << 1 << 0 << 0
                << 0 << 1 << 0
                << 0 << 0 << 1;
   
   m_inverseTransform << 1 << 0 << 0
                       << 0 << 1 << 0
                       << 0 << 0 << 1;
   
   rotate(rotateDegrees);
   scale(scaleXValue, scaleYValue);
   translate(translateXValue, translateYValue);
   pivot(m_pivot.x, m_pivot.y);
}

/**
 * Algorithme de tri rapide parallélisé par tâche
 * @param tab tableau à trié
 * @param len longueur du tableau
 */
void _quick_sort_omp(int *tab, int len)
{
	int i=0, j=len, ipivot;
	
	// pas de tri à faire
	if (len <= 1) 
		return;

	// pas assez d'éléments pour choisir un pivot
	if (len < 3) {
		if (tab[1] < tab[0])
			swap(&tab[1], &tab[0]);
		return;
	}
	
	// choix d'un pivot
	ipivot = pivot(tab, len);
	swap(&tab[0], &tab[ipivot]);

	while(1) {
		do i++; while (i < len && tab[i] < tab[0]);
		do j--; while (tab[j] > tab[0]);
		if (j < i) break;
		swap(&tab[i], &tab[j]);
	}

	swap(&tab[0], &tab[j]);

	#pragma omp task
	_quick_sort_omp(tab, j);
	
	#pragma omp task
	_quick_sort_omp(tab+j+1, len-j-1);
}

void CameraRig::setRotation(const glm::quat& transform_rotation) {
    // Get head transform (a child of camera rig object)
    Transform* transform = getHeadTransform();

    if (camera_rig_type_ == FREE) {
        transform->set_rotation(transform_rotation);
    } else if (camera_rig_type_ == YAW_ONLY) {
        glm::vec3 look_at = glm::rotate(transform_rotation,
                glm::vec3(0.0f, 0.0f, -1.0f));
        float yaw = atan2f(-look_at.x, -look_at.z) * 180.0f / M_PI;
        transform->set_rotation(
                glm::angleAxis(yaw, glm::vec3(0.0f, 1.0f, 0.0f)));
    } else if (camera_rig_type_ == ROLL_FREEZE) {
        glm::vec3 look_at = glm::rotate(transform_rotation,
                glm::vec3(0.0f, 0.0f, -1.0f));
        float pitch = atan2f(look_at.y,
                sqrtf(look_at.x * look_at.x + look_at.z * look_at.z)) * 180.0f
                / M_PI;
        float yaw = atan2f(-look_at.x, -look_at.z) * 180.0f / M_PI;
        transform->set_rotation(
                glm::angleAxis(pitch, glm::vec3(1.0f, 0.0f, 0.0f)));
        transform->rotateByAxis(yaw, 0.0f, 1.0f, 0.0f);
    } else if (camera_rig_type_ == FREEZE) {
        transform->set_rotation(glm::quat());
    } else if (camera_rig_type_ == ORBIT_PIVOT) {
        glm::vec3 pivot(getVec3("pivot"));
        transform->set_position(pivot.x, pivot.y,
                pivot.z + getFloat("distance"));
        transform->set_rotation(glm::quat());
        transform->rotateWithPivot(transform_rotation.w,
                transform_rotation.x, transform_rotation.y,
                transform_rotation.z, pivot.x, pivot.y, pivot.z);
    }
}

bool VideoThumbnailer::isFrameInteresting(const QImage &frame)
{
    float variance = 0;
    //taken from mplayerthumbs
    uint delta=0;
    uint avg=0;
    uint bytes=frame.numBytes();
    uint STEPS=bytes/2;
    QVarLengthArray<uchar> pivot(STEPS);

    const uchar *bits=frame.bits();
    // First pass: get pivots and taking average
    for( uint i=0; i<STEPS ; i++ ){
        pivot[i]=bits[i*(bytes/STEPS)];
        avg+=pivot[i];
    }
    avg=avg/STEPS;
    // Second Step: calculate delta (average?)
    for (uint i=0; i<STEPS; i++)
    {
        int curdelta=abs(int(avg-pivot[i]));
        delta+=curdelta;
    }
    variance= delta/STEPS;

    return variance > THRESHOLD_FRAME_VARIANCE;
}

dgMatrix dgMatrix::Symetric3by3Inverse () const
{
	dgMatrix copy(*this);
	dgMatrix inverse(dgGetIdentityMatrix());
	for (dgInt32 i = 0; i < 3; i++) {
		dgVector den(dgFloat32(1.0f) / copy[i][i]);
		copy[i] = copy[i].CompProduct4(den);
		inverse[i] = inverse[i].CompProduct4(den);
		for (dgInt32 j = 0; j < 3; j++) {
			if (j != i) {
				dgVector pivot(copy[j][i]);
				copy[j] -= copy[i].CompProduct4(pivot);
				inverse[j] -= inverse[i].CompProduct4(pivot);
			}
		}
	}
	
#ifdef _DEBUG
	dgMatrix test(*this * inverse);
	dgAssert(dgAbsf(test[0][0] - dgFloat32(1.0f)) < dgFloat32(0.01f));
	dgAssert(dgAbsf(test[1][1] - dgFloat32(1.0f)) < dgFloat32(0.01f));
	dgAssert(dgAbsf(test[2][2] - dgFloat32(1.0f)) < dgFloat32(0.01f));
#endif

	return inverse;
}

Eigen::VectorXf Simplex::run() {
	int piv_col, piv_row;
	while(1){
		piv_col = findPivotColumn();
		//std::cout << " CHOIX PIVOT " << std::endl;
		//std::cout << " simplexe run piv_col : "<< piv_col << std::endl;
		if(piv_col < 0)
		{
			//std::cout<< " STOP " << std::endl;
			getBest(); // optimal
			return best;
		}
		piv_row = findPivotRow(piv_col);
		//std::cout << " simplexe run piv_row : " << piv_row << std::endl;

		if(piv_row < 0)
		{
			//std::cout << " Pas de solution" << std::endl;
			break; //caca
		}
		//std::cout << " val pivot : " << tab(piv_row,piv_col);
		//std::cout << " " << std::endl;
		pivot(piv_row, piv_col);
		//std::cout << tab << std::endl;
	}
	return best;
}

uint MltPreview::imageVariance(QImage image)
{
    if (image.isNull()) return 0;
    uint delta = 0;
    uint avg = 0;
    uint bytes = image.numBytes();
    uint STEPS = bytes / 2;
    QVarLengthArray<uchar> pivot(STEPS);
    kDebug(DBG_AREA) << "Using " << STEPS << " steps\n";
    const uchar *bits=image.bits();
    // First pass: get pivots and taking average
    for( uint i=0; i<STEPS ; i++ ){
        pivot[i] = bits[2 * i];
#if QT_VERSION >= 0x040700
        avg+=pivot.at(i);
#else
        avg+=pivot[i];
#endif
    }
    avg=avg/STEPS;
    // Second Step: calculate delta (average?)
    for (uint i=0; i<STEPS; i++)
    {
#if QT_VERSION >= 0x040700
        int curdelta=abs(int(avg - pivot.at(i)));
#else
        int curdelta=abs(int(avg - pivot[i]));
#endif
        delta+=curdelta;
    }
    return delta / STEPS;
}

main ()
{
int n;
	MATRIX (A, MAX*MAX, MAX, MAX)

	read_matrix (A);

	if (DIM(A,0) != DIM(A,1))
	{
		printf ("La matrice doit etre carree\n");
		return;
	}
	n = DIM(A,0);

	MATRIX (B, MAX*MAX, n, n)

	printf ("Inversion de la matrice : ");
	print_matrix (A);

	pivot (A, B);

	printf ("Matrice inverse : ");
	print_matrix (B);

	ENDMAT ENDMAT
}

// Layer
bool CLwoWriter::WriteLayer()
{
	MSG_DEBUG("LAYR");

	// "LAYR" + size
	WriteChunk(CLwoFile::CHUNK_LAYR);

	// layer ID
	ushort layerID = 0; // hack, only support first layer
	WriteShort(layerID);

	// layer Flag
	ushort layerFlag = 0; // hack
	WriteShort(layerFlag);

	// layer Pivot
	Vector3D pivot(3); // Pivot
	pivot[0] = 0.0f;
	pivot[1] = 0.0f;
	pivot[2] = 0.0f;
	WriteVector3D(pivot);

	// layer Name
	WriteString(""); // Layer Name

	// no parent

	return true;
}

void forwardEliminationStep(double* localRows, float* pvtime)
{
	// Fill up this array with the ids of local rows. Will be used in the FE computation to quickly
	// check if indices of local rows are bigger than pivot row.
	int localRowIds[MAX_N];
	int i = 0;
	int j;
	for(j = 0; j < N; j++)
	{
		if(getRowPID(j) == PID) 
		{
			localRowIds[i] = j;	
			i++;
		}
	}
	if(i != RPP) printf("local row number %d != RPP %d!!\n", i, RPP); // This should never happen.
	
	for(j = 0; j < N; j++)
	{
		double rowj[MAX_N];
		double* rowPtr = getLocalRow(j, localRows);
		
		int rowSource = getRowPID(j);
		
		// Pivot
		pivot(localRows, rowPtr, j, rowSource, localRowIds, pvtime);
		
		if(rowPtr != NULL)
		{
			memcpy(rowj, rowPtr, (N + 1) * sizeof(double));
		}
		
		// Broadcast the row.
		MPI_Bcast(rowj, N + 1, MPI_DOUBLE, rowSource, MPI_COMM_WORLD);
		
		// Compute new coefficients for rows after j
		int i;
		for(i = 0; i < RPP; i++)
		{
			double* rowi = &localRows[i * (N + 1)];
			double mult = -(rowi[j]/rowj[j]);
			//printf("NODE %d LOCAL ROW %d FIRST VAL %11.4e\n", PID, i, rowi[0]);
			
			if(localRowIds[i] > j)
			{
				int k;
				for(k = 0; k < N + 1; k++)
				{
					if(k == j) rowi[k] = 0;
					else rowi[k] = mult * rowj[k] + rowi[k];
					//double v = mult * rowj[k] + rowi[k];
					
					// Kind of hack but useful for visually debugging output matrix.
					//if(rowi[k] < 0.00000000000001) rowi[k] = 0;
				}
			}
		}
	}
}

  m2::RectD SymbolElement::GetBoundRect() const
  {
    // Skip for one pixel on every border.
    m2::PointD const sz(m_symbolRect.SizeX() - 2, m_symbolRect.SizeY() - 2);
    m2::PointD const posPt = computeTopLeft(sz, pivot(), position());

    return m2::RectD(posPt, posPt + sz);
  }

void QuickSort<T>::sort(Iterator begin, Iterator end)
{
    typename QuickSort<T>::Iterator m = pivot(begin, end);
    if (m - begin > 1)
        sort(begin, m);
    if (end - m > 1)
        sort(m + 1, end);
}