void Model_cell::run() {
    if (this->timeCounter == 0 || ((this->timeCounter + 1) % trajOutputInterval == 0)) {
        this->outputTrajectory(this->trajOs);
    }
    Eigen::MatrixXd randomDispMat;
    randomDispMat.setRandom(3, numP);
    
    calForces();
    for (int i = 0; i < numP; i++) {
//        cellInfo[i]->polarization.fill(-1);
//        int oldIdx = particles[i]->meshFaceIdx;
//                if (i == 146)
//            std::cerr << "polarization"<<cellInfo[i]->polarization << std::endl;
        particles[i]->vel = cellInfo[i]->polarization*polar_vel + particles[i]->F;
        cellInfo[i]->polarization *= (1 - dt_ / tau);
        cellInfo[i]->polarization += sqrt(2.0*sigma*dt_) * randomDispMat.col(i) + (beta * cellInfo[i]->inhibition)*dt_;

//            for (int j = 0; j < 3; j++){
//        if (std::isnan(cellInfo[i]->polarization[j])){
//            std::cerr << "particle vel NaN at step" << this->timeCounter << "\t" << i << std::endl;
//            std::cerr << particles[i]->r << std::endl;
//            std::cerr << particles[i]->vel << std::endl;
//            std::cerr << "polarization"<<cellInfo[i]->polarization << std::endl;
//             std::cerr <<"force" <<particles[i]->F << std::endl;
//             std::cerr << cellInfo[i]->inhibition << std::endl;
//             std::cerr << randomDispMat.col(i) << std::endl;
             
//            exit(1);
//        }
//        }

        this->moveOnMesh(i);
    }
    this->timeCounter++;
}
void Model::run() {
    if (this->timeCounter == 0 || ((this->timeCounter + 1) % trajOutputInterval == 0)) {
        this->outputTrajectory(this->trajOs);
    }
    
    calForces();
    Eigen::MatrixXd randomDispMat;
    randomDispMat.setRandom(3,numP);
//    omp_set_num_threads(1);
#pragma omp parallel default(shared)
{//    std::cout << omp_get_num_threads() << std::endl;
#pragma omp for schedule(dynamic)
    for (int i = 0; i < numP; i++) {            
//            Eigen::Vector3d velocity;
            for (int j = 0; j < 3; j++){
//                particles[i]->F(j) = 0.0;
                particles[i]->vel(j) = diffusivity_t * particles[i]->F(j) + sqrt(2.0 * diffusivity_t/dt_) * randomDispMat(j,i);
            }
//            particles[i]->vel = velocity;
            /* Obtain thread number */
//            int tid = omp_get_thread_num();
//            printf("Hello World from thread = %d\n", tid);
//            std::cout << omp_get_num_threads() << std::endl;
//            std::cout << numP << std::endl;
//            std::cout << velocity << std::endl;
            this->moveOnMesh(i);
        }

}    
//    this->moveOnMesh_OMP();
    this->timeCounter++;
    
}
void Model::run() {
    if (this->timeCounter == 0 || ((this->timeCounter + 1) % trajOutputInterval == 0)) {
        this->outputTrajectory(this->trajOs);
    }
    Eigen::MatrixXd randomDispMat;
    randomDispMat.setRandom(3,numP);
    calForces(); 
        for (int i = 0; i < numP; i++) {            
            Eigen::Vector3d velocity;
            for (int j = 0; j < 3; j++){
               velocity(j) = diffusivity_t * particles[i]->F(j) + sqrt(2.0 * diffusivity_t/dt_) * randomDispMat(j,i);
            }           
            this->moveOnMesh(i);
        }
    this->timeCounter++;
    
}
void IRLTest::generateData()
{
  int num_active_features;
  int num_data;
  int num_samples_per_data;
  double cost_noise_stddev;

  num_features_ = 10;
  num_active_features = 2;
  num_data = 10;
  num_samples_per_data = 10;
  cost_noise_stddev = 0.1;

  // generate random weights
  real_weights_ = Eigen::VectorXd::Zero(num_features_);
  real_weights_.head(num_active_features).setRandom();

  data_.resize(num_data);
  for (int i=0; i<num_data; ++i)
  {
    IRLData* d = new IRLData(num_features_);
    Eigen::MatrixXd features = Eigen::MatrixXd::Zero(num_samples_per_data, num_features_);
    Eigen::VectorXd target = Eigen::VectorXd::Zero(num_samples_per_data);
    features.setRandom();

    // compute w^t * phi
    Eigen::VectorXd costs = features*real_weights_;

    // add random noise to costs
    costs += cost_noise_stddev * Eigen::VectorXd::Random(num_samples_per_data);

    // set min cost target to 1
    int min_index;
    double min_cost = costs.minCoeff(&min_index);
    target(min_index) = 1.0;
    d->addSamples(features, target);
    data_[i].reset(d);
  }
  real_weights_exist_ = true;
}