void 
LPC::process(fvec& in, fvec& out)
{
//    fprintf( stderr, "%i %i %i %i\n", in.size(), inSize_, out.size(), outSize_ );
  assert((in.size() == inSize_) && (out.size() == outSize_));
  unsigned int i,j;
  autocorr_->process(in, corr_);
  pitch_ = autocorr_->pitch();
  for (i=1; i<order_; i++)
    for (j=1; j<order_; j++)
      {
		rmat_(i-1,j-1) = corr_(abs((int)(i-j)));
      }

  rmat_.invert(temp_);
  for (i=0; i < order_-1; i++)
    {
      out(i) = 0.0;
      for (j=0; j < order_-1; j++)
	    {
		  out(i) += (rmat_(i,j) * corr_(1+j));
	    }
	}

  predict(in, out);
  out(order_-1) = pitch_;
  out(order_) = power_;
}

int RandFourierMap(int nKernelType, const fvec& x, const std::vector<fvec> &W, const fvec &b, fvec &result)
{
    int nErrCode = 0;
    if(W.empty() || b.empty())
    {
        return 1;
    }
    int nKernelRank = W[0].size();
    switch(nKernelType)
    {
    case RAND_KERNEL_RBF:
        for(int rank_idx = 0; rank_idx < nKernelRank; ++rank_idx)
        {
            float sum = 0.0;
            for(int dim_idx = 0; dim_idx < x.size(); ++dim_idx)
            {
                sum += W[dim_idx][rank_idx] * x[dim_idx];
            }
            result.push_back(sqrt(2.0 / nKernelRank) * cos(sum + b[rank_idx]));
        }
        break;
    default:
        nErrCode = 1;
    }

    return nErrCode;
}

int RandFourierFactorize(int nKernelType, int nKernelRank, float fGamma, const std::vector<fvec>& X, std::vector<fvec> &G, std::vector<fvec> &W, fvec &b)
{
    int nErrCode = 0;

    int m = X.size();
    if(m == 0)
    {
        return nErrCode;
    }

    int dim = X[0].size();
    G.clear();
    W.clear();
    b.clear();

    //generate random features

    dlib::rand r;
    std::ostringstream seed;
    seed << (unsigned int) time(0);
    r.set_seed(seed.str());

    switch(nKernelType)
    {
    case RAND_KERNEL_RBF:

        for(int ind = 0; ind < dim; ++ind)
        {
            fvec tmp_vec;
            for(int rank_idx = 0; rank_idx < nKernelRank; ++rank_idx)
            {
                tmp_vec.push_back(r.get_random_gaussian() * fGamma);
            }
            W.push_back(tmp_vec);
        }
        for(int ind = 0; ind < nKernelRank; ++ind)
        {
            b.push_back(r.get_random_float() * 2.0 * PI);
        }
        for(int ind = 0; ind < m; ++ind)
        {
            fvec tmp_vec;
            RandFourierMap(nKernelType, X[ind], W, b, tmp_vec);
            G.push_back(tmp_vec);
        }
        break;

    default:
        //unknown kernel type
        //printf("unknown type\n");
        nErrCode = 1;
        break;
    };

    return nErrCode;
}

void ClassMLP::SetParams(Classifier *classifier, fvec parameters)
{
    if(!classifier) return;
    float alpha = parameters.size() > 0 ? parameters[0] : 1;
    float beta = parameters.size() > 1 ? parameters[1] : 1;
    int layers = parameters.size() > 2 ? parameters[2] : 1;
    int neurons = parameters.size() > 3 ? parameters[3] : 1;
    int activation = parameters.size() > 4 ? parameters[4] : 0;

    ((ClassifierMLP *)classifier)->SetParams(activation, neurons, layers, alpha, beta);
}

void
DownSampler::process(fvec& in, fvec& out)
{
  assert((in.size() == inSize_) && (out.size() == outSize_));  
  unsigned int size = in.size();
  unsigned int i;
  
  for (i=0; i< size / factor_; i++)
    {
      out(i) = in(i * factor_);
    }
}

fvec ClustererDBSCAN::Test( const fvec &sample)
{
    fvec res;
    res.resize(nbClusters+1,0);

    //convert input to Point
    Point v (sample.size());
    for (int i = 0; i < sample.size(); ++i)
    {
        v(i)=sample[i];
    }

    // find the nearest point in our samples
    int nearest = -1;
    double dist = INFINITY;
    double temp_d = 0;

    if (_type==0) //if DBSCAN we set _depth like _eps
    {
        _depth=_eps;
    }
    for (int j = 0; j < pts.size(); ++j)
    {
        // according to the selected metric
        if(_metric == 0)
        {
            Metrics::Distance<Metrics::Cosine<Point> > d;
            temp_d = d.distance(v, pts[j]);
        }
        else
        {
            Metrics::Distance<Metrics::Euclidean<Point> > d;
            temp_d = d.distance(v, pts[j]);
        }
        if (temp_d < dist && temp_d < _eps && _pointId_to_clusterId[j] > 0 && _core[j]){
            dist = temp_d;
            nearest = j;
        }
    }

    // did we find something?
    if (nearest > -1){
        if (dist < _depth){ // is it near enough?
            res[_pointId_to_clusterId[nearest]-1] = 1; //take the color of that cluster
        }
        else if (abs(dist - _eps) < _eps*0.01) //in OPTICS, we are at the border of _eps : draw a thin line, darker
        {
            res[_pointId_to_clusterId[nearest]-1] = 0.5;
        }
    }

    return res;
}

void operator /= (fvec &a, const float b)
{
	if (a.size() == 2)
	{
		a[0] /= b;
		a[1] /= b;
	}
	else
	{
		for (int i = 0; i < a.size(); i++) a[i] /= b;
	}
}

void 
MFCC::process(fvec& in, fvec& out)
{
  unsigned int i,k;
  if ((in.size() != inSize_) || (out.size() != outSize_))
    {
      cerr << "Warnging: MFCC::process:  inSize_ and input window size do not agree" << endl;
      return;
    }  
  
  hamming_->process(in, windowed);
  magfft_->process(windowed, magnitude);

  for (i=0; i < inSize_/2; i++)
    fmagnitude(i) = magnitude(i);
  
  for (i=0; i< inSize_/2; i++)
    fmagnitude(i+ inSize_/2) = fmagnitude(inSize_/2 - i);
  
  float sum =0.0;
  // Calculate the filterbank responce
  for (i=0; i<totalFilters_; i++)
    { 
      sum = 0.0;
      for (k=0; k<fftSize_; k++)
	{
	  sum += (mfccFilterWeights_(i, k) * fmagnitude(k));
	}
      if (sum != 0.0)
	earMagnitude_(i) = log10(sum);
      else 
	earMagnitude_(i) = 0.0;
    }  

  // Take the DCT 
  for (i=0; i < cepstralCoefs_; i++)
    {
      sum =0.0;
      for (k=0; k < totalFilters_; k++)
	{
	  sum += (mfccDCT_(i,k) * earMagnitude_(k));
	}
      out(i) = sum;
    }  
  

}

// SANITY TEST
void test_coverage(const fvec& lower, const fvec& upper, const vector<Rectangle>& recs, uint nsamples)
{
    // first, test the rectangles
    uint N = lower.size();
    for (uint r = 0; r < recs.size(); r++)
    {
        for (uint k = 0; k < N; k++)
        {
            if (recs[r].center[k] != recs[r].lb[k] + (recs[r].ub[k]-recs[r].lb[k])/2)
            {
                cout << "***** rec " << r << " expected center at " << recs[r].center[k] << ", got " << recs[r].lb[k] + (recs[r].ub[k]-recs[r].lb[k])/2 << endl;
            }
            
            if (recs[r].lb[k] < 0.0)
                cout << "***** rec " << r << " invalid lower bound " << recs[r].lb[k] << "!" << endl;
            if (recs[r].ub[k] > 1.0)
                cout << "***** rec " << r << " invalid upper bound " << recs[r].ub[k] << "!" << endl;
        }
    }
    
    for (uint i = 0; i < nsamples; i++)
    {
        uint hits = 0;
        
        fvec rvec = fvec(N, 0.0);
        srand (i);
        for (uint j = 0; j < N; j++) rvec[j] = double(rand()) / double(RAND_MAX);
        
        for (uint r = 0; r < recs.size(); r++)
        {
            if (DEBUG) 
            {
                cout << "\t check [";
                for (uint k = 0; k < N; k++) cout << " " << recs[r].lb[k];
                cout << " ]" << endl;
                for (uint k = 0; k < N; k++) cout << " " << recs[r].ub[k];
                cout << " ]" << endl;
            }
            
            bool found = true;
            for (uint k = 0; k < N; k++)
            {
                if (rvec[k] < recs[r].lb[k] || rvec[k] > recs[r].ub[k])
                {
                    found = false;
                    break;
                }
            }
            if (found) hits += 1;
        }
        
        if (hits != 1)
        {
            cout << "random vector [";
            for (uint k = 0; k < N; k++) cout << " " << rvec[k];
            cout << " ] has " << hits << " hits!" << endl;
        }
    }
}

Rectangle::Rectangle(const fvec& lower, const fvec& upper, Direct* D)
{
    lb = fvec(lower);
    ub = fvec(upper);

    center = fvec(lower.size(), 0.0);
    d = 0.0;
    for (unsigned int i = 0; i < lower.size(); i++)
    {
        center[i] = lb[i] + (ub[i]-lb[i]) / 2.;
        d += pow((lb[i]-center[i]), 2);
    }
    d = sqrt(d);
    // if (DEBUG) cout << "***** d = " << d << endl;
    
    y = D->samplef(center);
}

void heapSort(fvec &height, ivec &index, int tsize)
{
    int i = 0;
    float ftemp = 0.0;
    int itemp = 0;
    int size = index.size();

    if (tsize != height.size())
    {
        tsize = height.size();
    }
    else
        ;

    if (tsize != size)
    {
        cout << "\nfuck you\n" << flush;
    }
    else
        ;

    i = tsize;
    i /= 2;
    i -= 1;
    for (; i >= 0; i--)
    {
        siftDown(height, index, i, tsize);
    }

    i = tsize - 1;
    for (; i >= 1; i--)
    {
        ftemp = height[0];
        height[0] = height[i];
        height[i] = ftemp;

        itemp = index[0];
        index[0] = index[i];
        index[i] = itemp;

        siftDown(height, index, 0, i - 1);
    }
}

void 
RMS::process(fvec& in, fvec& out) 
{
  float rmsEnergy = 0.0;
  unsigned int i;
  
  if ((in.size() != inSize_) || (out.size() != outSize_))
    {
      cerr << "Warning: RMS::process: inSize_ and input window size do not agree" << endl;
      return;
    }
  float val;
  /* Compute centroid using moments */
  for (i=0; i < inSize_; i++)
    {
      val = in(i);
      rmsEnergy += (val * val);
    }
  rmsEnergy /= inSize_;
  rmsEnergy = sqrt(rmsEnergy);
  out(0) = rmsEnergy;
}

//returns 2*area( triangle(a, b ,c) )
float doubleArea(const fvec &a, const fvec &b, const fvec &c)
{
    int asize = a.size();
    int bsize = b.size();
    int csize = c.size();

    float da = 0;

    if ((asize == 3) && (asize == bsize) && (asize == csize))
    {
        float cyclic = (a[0] * b[1] * c[2] + a[1] * b[2] * c[0] + a[2] * b[0] * c[1]);
        float anti_cyclic = (a[0] * b[2] * c[1] + a[1] * b[0] * c[2] + a[2] * b[1] * c[0]);

        da = (cyclic - anti_cyclic);
    }
    else if ((asize == 2) && (asize == bsize) && (asize == csize))
    {
        da = (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]);
    }
    else
        ;

    return da;
}

void PFilterSIRCUDA::update(fvec measurement) {
    float neff=0.0;
    float* measurementDev;
    cudaMalloc( &measurementDev, (size_t) measurement.n_rows * sizeof(float)) ;
    cudaMemcpy(measurementDev,measurement.memptr(),(size_t) measurement.n_rows * sizeof(float), cudaMemcpyHostToDevice);

    //fmat virtualMeasurementOfParticles;
    //fmat differences = zeros<fmat>(measurement.n_rows,particles.samples.n_cols);
    frowvec evals;

    //virtualMeasurementOfParticles = process->hfun(&particles.samples);
    measurementOnGPU = process->hfun_gpu(samplesOnGPU, particles.samples.n_cols, particles.samples.n_rows);

    // calculate differences
    //differences = virtualMeasurementOfParticles - inputMatrix;
    /*for (unsigned int i=0; i< virtualMeasurementOfParticles.n_cols; ++i)
    {
    	for(unsigned int j=0; j<virtualMeasurementOfParticles.n_rows;++j)
    	{
    		differences(j,i) = virtualMeasurementOfParticles(j,i) - measurement(j);
    	}
    }*/

    callDeviationKernel(measurementOnGPU, measurementDev, particles.samples.n_rows,
                        particles.samples.n_cols, deviationsOnGPU);

    evals = process->eval_gpu(deviationsOnGPU, particles.samples.n_cols);

    // % is the Schur product (elementwise vector multiplication
    particles.weights = particles.weights % evals;

    // get samples from graphics card
    cudaMemcpy(particles.samples.memptr(),samplesOnGPU, particles.samples.n_elem * sizeof(float), cudaMemcpyDeviceToHost);

    normalizeWeights();

    neff=calculateNeff();

    if (neff <= nthr) {
#ifdef VERBOSE
        printf("too few particles. 1/N for all particles\n");
#endif
        particles.weights = ones<frowvec>(particles.weights.n_cols) / (float)particles.weights.n_cols;
    }
    else    particles = resampler->resample(&particles);
}

//function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
void BFilterUKF::utProcess(fmat X,fvec Wm, fvec Wc, unsigned int n, fmat R)
{
    //Unscented Transformation
    //Input:
    //        f: nonlinear map
    //        X: sigma points
    //       Wm: weights for mean
    //       Wc: weights for covraiance
    //        n: numer of outputs of f
    //        R: additive covariance
    //Output:
    //        y: transformed mean
    //        Y: transformed smapling points
    //        P: transformed covariance
    //       Y1: transformed deviations

    unsigned int L=X.n_cols;
    x1 = zeros<fvec>(n);
    X1 = zeros<fmat>(n,L);
    //for k=1:L
    for (unsigned int k=0; k < L; ++k)
    {
        fmat XColK = X.col(k);
        X1.col(k)= process->ffun(&XColK);
        x1=x1+Wm(k)*X1.col(k);
    }

    //X2=X1-x1(:,ones(1,L));  // generate duplicates of vector x1
    X2 = X1;
    for (unsigned int j = 0; j < L; ++j)
    {
        for (unsigned int i = 0; i < x1.n_rows; ++i)
        {
            X2(i,j) -= x1(i);
        }
    }

    P1=X2*Wc.diag()*X2.t()+R;
}

//function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
void BFilterUKF::utMeasurement(fmat X, fvec Wm, fvec Wc, unsigned int n, fmat R)
{
    //Unscented Transformation
    //Input:
    //        f: nonlinear map
    //        X: sigma points
    //       Wm: weights for mean
    //       Wc: weights for covraiance
    //        n: numer of outputs of f
    //        R: additive covariance
    //Output:
    //        y: transformed mean
    //        Y: transformed smapling points
    //        P: transformed covariance
    //       Y1: transformed deviations

    unsigned int L=X.n_cols;
    z1=zeros<fvec>(n);
    Z1=zeros<fmat>(n,L);
    //for k=1:L
    for (unsigned int k=0; k < L; ++k)
    {
        fmat XColK = X.col(k);
        Z1.col(k)= process->ffun(&XColK);
        z1=z1+Wm(k)*Z1.col(k);
    }

    //Z2=Z1-x1(:,ones(1,L));
    Z2 = Z1;
    for (unsigned int j = 0; j < L; ++j)
    {
        for (unsigned int i = 0; i < x1.n_rows; ++i)
        {
            Z2(i,j) -= x1(i);
        }
    }

    P2=Z2*Wc.diag()*Z2.t()+R;
}

void search2(fvec &stat, ivec &sel_id, int &marg_id, const fvec &S, const fmat &Sigma, const int &mc){
  
  int ns = S.size();
  
	//find best marginal SNP
	int id = -1;
	double max_stat = -1.0;
	for(int j = 0; j < ns; ++j){
		double s = S[j] * S[j] / Sigma[j][j];
		if(s > max_stat){
			max_stat = s;
			id = j;
		}
	}
	
  stat = fvec(1, max_stat);
  assert(id >= 0 && id < ns);
  sel_id = ivec(1, id);
  marg_id = id;
  
	if(ns == 1){
		return;
	}
  
  //find best pair
  int id1 = -1;
  int id2 = -1;
  max_stat = -1.0;
  for(int j = 0; j < ns; ++j){
    for(int k = j + 1; k < ns; ++k){
      double a = Sigma[j][j];
      double b = Sigma[j][k];
      double d = Sigma[k][k];
      double stat = (d * S[j] * S[j] - 2 * b * S[j] * S[k] + a * S[k] * S[k]) / (a * d - b * b);
      if(stat > max_stat){
        max_stat = stat;
        id1 = j;
        id2 = k;
      }
    }
  }
  
  stat.push_back(max_stat);
  assert(id1 >= 0 && id1 < ns);
  assert(id2 >= 0 && id2 < ns);
  sel_id = ivec(1, id1);
  sel_id.push_back(id2);
  
  if(ns == 2){
    return;
  }
	
	dmat inv_Sigma22(mc + 1, dvec(mc + 1, .0));
  double a = Sigma[id1][id1];
  double b = Sigma[id1][id2];
  double d = Sigma[id2][id2];
  double e = a * d - b * b;
	inv_Sigma22[0][0] = d / e;
  inv_Sigma22[0][1] = -b / e;
  inv_Sigma22[1][0] = inv_Sigma22[0][1];
  inv_Sigma22[1][1] = a / e;
	
	dvec S2(mc + 1, .0);
	S2[0] = S[id1];
  S2[1] = S[id2];
	
	vector<bool> used(ns, false);
	used[id1] = true;
  used[id2] = true;
	
	double w = max_stat;
	
	for(int j = 2; j <= mc; ++j){//j is the number of SNPs being selected
		
		dvec v(j, .0); // v = (Sigma22)^-1 S2
		for(int i = 0; i < j; ++i){
			v[i] = .0;
			for(int k = 0; k < j; ++k){
				v[i] += inv_Sigma22[i][k] * S2[k];
			}
		}
		
		max_stat = -1.0;
		double max_qf2 = -1.0;
		id = -1;
		for(int k = 0; k < ns; ++k){
			if(used[k]){
				continue;
			}
			
			double S1 = S[k];
			double qf1 = .0; //qf1 = sigma12 (Sigma22)^-1 S2
			double qf2 = .0; //qf2 = sigma12 (Sigma22)^-1 sigma21
			for(int l = 0; l < j; ++l){
				qf1 += Sigma[k][sel_id[l]] * v[l];
				double tmp = .0; // tmp = the l-th column of sigma12 (Sigma22)^-1
				for(int t = 0; t < j; ++t){
					tmp += Sigma[k][sel_id[t]] * inv_Sigma22[t][l];
				}
				qf2 += tmp * Sigma[sel_id[l]][k];
//.........这里部分代码省略.........

// a little function which copies content from the fvec to a classic array of floats.
// this requires that ret is properly allocated!
void ConvertToRawArray(const fvec &a,float * ret){
    int dim = a.size();
    FOR(i,dim){
        ret[i] = a[i];
    }
}