void ProbabilisticAnchorGraph::set_particle_probabilities_on_anchors(
                         Particle *p,
                         FittingSolutionRecords sols) {
  IMP_USAGE_CHECK(sols.size()>0,
                  "no solutions provided\n");
  IMP_NEW(algebra::NearestNeighborD<3>, nn, (positions_));
  Ints anchor_counters;
  anchor_counters.insert(anchor_counters.end(),positions_.size(),0);
  for (unsigned int i=0;i<sols.size();i++) {
    algebra::Vector3D loc=
      sols[i].get_fit_transformation().get_transformed(
                               core::XYZ(p).get_coordinates());
    anchor_counters[nn->get_nearest_neighbor(loc)]++;
  }
  Floats probs;
  for (unsigned int i=0;i<anchor_counters.size();i++) {
    probs.push_back(1.*anchor_counters[i]/sols.size());
  }
  particle_to_anchor_probabilities_[p]=probs;
}

double EzRestraint::unprotected_evaluate(DerivativeAccumulator *da) const {
  IMP_UNUSED(da);
  Model *m = get_model();
  // check if derivatives are requested
  IMP_USAGE_CHECK(!da, "Derivatives not available");

  double score = 0.0;
  for (unsigned i = 0; i < ps_.size(); ++i) {
    //   if(da){
    //    FloatPair score_der =
    //    ufs_[i]->evaluate_with_derivative
    //(fabs(core::XYZ(ps_[i]).get_coordinate(2)));
    //    score += score_der.first;
    //    core::XYZ(ps_[i]).add_to_derivative(2,score_der.second,*da);
    //   }else{
    score += ufs_[i]->evaluate(fabs(core::XYZ(m, ps_[i]).get_coordinate(2)));
    //   }
  }
  return score;
}

void ModelObject::validate_outputs() const {
  if (!get_has_dependencies()) return;
  IMP_IF_CHECK(USAGE_AND_INTERNAL) {
    IMP_CHECK_OBJECT(this);
    ModelObjectsTemp ret = do_get_outputs();
    std::sort(ret.begin(), ret.end());
    ret.erase(std::unique(ret.begin(), ret.end()), ret.end());
    ModelObjectsTemp saved = get_model()->get_dependency_graph_outputs(this);
    std::sort(saved.begin(), saved.end());
    ModelObjectsTemp intersection;
    std::set_intersection(saved.begin(), saved.end(), ret.begin(), ret.end(),
                          std::back_inserter(intersection));
    IMP_USAGE_CHECK(
        intersection.size() == ret.size(),
        "Dependencies changed without invalidating dependencies."
            << " Make sure you call set_has_dependencies(false) any "
            << "time the list of dependencies changed. Object is " << get_name()
            << " of type " << get_type_name());
  }
}

IMPEM_BEGIN_NAMESPACE

void XplorReaderWriter::read(const char *filename, float **data,
                            DensityHeader &header)
{
  std::ifstream XPLORstream(filename);
  //header
  internal::XplorHeader xheader;
  read_header(XPLORstream,xheader);
  xheader.GenerateCommonHeader(header);

  //data
  int size = xheader.extent[0]*xheader.extent[1]*xheader.extent[2];
  *data =  new float[size];
  IMP_USAGE_CHECK(*data,
            "XplorReader::read can not allocated space for data - the "
                  << "requested size: " << size * sizeof(float));
  read_map(XPLORstream, *data, xheader);
  XPLORstream.close();
}

// write approximate function, remove rigid bodies for intermediates
core::RigidBody create_rigid_body(const Hierarchies& h,
                                  std::string name) {
  if (h.empty()) return core::RigidBody();
  for (unsigned int i=0; i< h.size(); ++i) {
    IMP_USAGE_CHECK(h[i].get_is_valid(true), "Invalid hierarchy passed.");
  }
  Particle *rbp= new Particle(h[0]->get_model());
  rbp->set_name(name);
  ParticlesTemp all;
  for (unsigned int i=0; i< h.size(); ++i) {
    ParticlesTemp cur= rb_process(h[i]);
    all.insert(all.end(), cur.begin(), cur.end());
  }
  core::RigidBody rbd
    = core::RigidBody::setup_particle(rbp, core::XYZs(all));
  rbd.set_coordinates_are_optimized(true);
  for (unsigned int i=0; i< h.size(); ++i) {
    IMP_INTERNAL_CHECK(h[i].get_is_valid(true), "Invalid hierarchy produced");
  }
  return rbd;
}

void MolecularDynamicsMover::save_coordinates()
{
    IMP_OBJECT_LOG;
    kernel::ParticlesTemp ps = md_->get_simulation_particles();
    unsigned nparts = ps.size();
    coordinates_.clear();
    coordinates_.reserve(nparts);
    velocities_.clear();
    velocities_.reserve(nparts);
    for (unsigned i=0; i<nparts; i++)
    {
        bool isnuisance = Nuisance::get_is_setup(ps[i]);
        bool isxyz = core::XYZ::get_is_setup(ps[i]);
        IMP_USAGE_CHECK(isnuisance||isxyz,
                "Particle " << ps[i] << " is neither nuisance nor xyz!");
        if (isnuisance)
        {
            std::vector<double> x(1,
                    Nuisance(ps[i]).get_nuisance());
            coordinates_.push_back(x);
            std::vector<double> v(1,
                    ps[i]->get_value(FloatKey("vel")));
            velocities_.push_back(v);
        }
        if (isxyz)
        {
            std::vector<double> coords;
            core::XYZ d(ps[i]);
            coords.push_back(d.get_coordinate(0));
            coords.push_back(d.get_coordinate(1));
            coords.push_back(d.get_coordinate(2));
            coordinates_.push_back(coords);
            std::vector<double> v;
            v.push_back(ps[i]->get_value(FloatKey("vx")));
            v.push_back(ps[i]->get_value(FloatKey("vy")));
            v.push_back(ps[i]->get_value(FloatKey("vz")));
            velocities_.push_back(v);
        }
    }
}

SimpleConnectivity create_simple_connectivity_on_molecules(
                   const atom::Hierarchies &mhs)
{
  size_t mhs_size = mhs.size();

  IMP_USAGE_CHECK(mhs_size > 0, "At least one hierarchy should be given");

  kernel::ParticlesTemp ps;

  for ( size_t i=0; i<mhs_size; ++i )
  {
    ps.push_back(mhs[i].get_particle());
  }

  /****** Define Refiner ******/
  // Use LeavesRefiner for the hierarchy leaves under a particle

  IMP_NEW(core::LeavesRefiner, lr, (atom::Hierarchy::get_traits()));

  /****** Define PairScore ******/
  // Score on the lowest of the pairs defined by refining the two particles.

  IMP_NEW(core::HarmonicUpperBound, h, (0, 1));
  IMP_NEW(core::SphereDistancePairScore, sdps, (h));
  IMP_NEW(core::KClosePairsPairScore, lrps, (sdps, lr));

  /****** Set the restraint ******/

  IMP_NEW(core::ConnectivityRestraint, cr, (lrps));
  cr->set_particles((ps));

  /****** Add restraint to the model ******/

  //Model *mdl = mhs[0].get_particle()->get_model();
  //mdl->add_restraint(cr);

  /****** Return a SimpleConnectivity object ******/

  return SimpleConnectivity(cr, h, sdps);
}

IMPISD_BEGIN_NAMESPACE

GaussianProcessInterpolation::GaussianProcessInterpolation(
    FloatsList x, Floats sample_mean, Floats sample_std, unsigned n_obs,
    UnivariateFunction *mean_function, BivariateFunction *covariance_function,
    Particle *sigma, double sparse_cutoff)
    : Object("GaussianProcessInterpolation%1%"),
      x_(x),
      n_obs_(n_obs),
      mean_function_(mean_function),
      covariance_function_(covariance_function),
      sigma_(sigma),
      cutoff_(sparse_cutoff) {
  // O(M)
  // store dimensions
  M_ = x.size();
  N_ = x[0].size();
  sigma_val_ = Scale(sigma_).get_nuisance();
  // basic checks
  IMP_USAGE_CHECK(sample_mean.size() == M_,
                  "sample_mean should have the same size as x");
  IMP_USAGE_CHECK(sample_std.size() == M_,
                  "sample_std should have the same size as x");
  IMP_USAGE_CHECK(mean_function->get_ndims_x() == N_,
                  "mean_function should have " << N_ << " input dimensions");
  IMP_USAGE_CHECK(mean_function->get_ndims_y() == 1,
                  "mean_function should have 1 output dimension");
  IMP_USAGE_CHECK(covariance_function->get_ndims_x1() == N_,
                  "covariance_function should have "
                      << N_ << " input dimensions for first vector");
  IMP_USAGE_CHECK(covariance_function->get_ndims_x2() == N_,
                  "covariance_function should have "
                      << N_ << " input dimensions for second vector");
  IMP_USAGE_CHECK(covariance_function->get_ndims_y() == 1,
                  "covariance_function should have 1 output dimension");
  // set all flags to false = need update.
  force_mean_update();
  force_covariance_update();
  // compute needed matrices
  compute_I(sample_mean);
  compute_S(sample_std);
}

IMPEM_BEGIN_NAMESPACE

EnvelopePenetrationRestraint::EnvelopePenetrationRestraint(Particles ps,
                                                           DensityMap *em_map,
                                                           Float threshold)
    : Restraint(ps[0]->get_model(), "Envelope penetration restraint") {
  IMP_LOG_TERSE("Load envelope penetration with the following input:"
                << "number of particles:" << ps.size() << "\n");
  threshold_ = threshold;
  target_dens_map_ = em_map;
  IMP_IF_CHECK(USAGE) {
    for (unsigned int i = 0; i < ps.size(); ++i) {
      IMP_USAGE_CHECK(core::XYZR::get_is_setup(ps[i]),
                      "Particle " << ps[i]->get_name() << " is not XYZR"
                                  << std::endl);
    }
  }
  add_particles(ps);
  ps_ = ps;
  IMP_LOG_TERSE("after adding particles" << std::endl);
  IMP_LOG_TERSE("Finish initialization" << std::endl);
}

core::RigidBody setup_as_rigid_body(Hierarchy h) {
  IMP_USAGE_CHECK(h.get_is_valid(true),
                  "Invalid hierarchy passed to setup_as_rigid_body");
  IMP_WARN("create_rigid_body should be used instead of setup_as_rigid_body"
           << " as the former allows one to get volumes correct at coarser"
           << " levels of detail.");
  core::RigidBody rbd = core::RigidBody::setup_particle(h, get_leaves(h));
  rbd.set_coordinates_are_optimized(true);
  kernel::ParticlesTemp internal = core::get_internal(h);
  for (unsigned int i = 0; i < internal.size(); ++i) {
    if (internal[i] != h) {
      core::RigidMembers leaves(get_leaves(Hierarchy(internal[i])));
      if (!leaves.empty()) {
        algebra::ReferenceFrame3D rf = core::get_initial_reference_frame(
            get_as<kernel::ParticlesTemp>(leaves));
        core::RigidBody::setup_particle(internal[i], rf);
      }
    }
  }
  IMP_INTERNAL_CHECK(h.get_is_valid(true), "Invalid hierarchy produced");
  return rbd;
}

IMPALGEBRA_BEGIN_NAMESPACE

Sphere3D get_enclosing_sphere(const Sphere3Ds &ss) {
  IMP_USAGE_CHECK(!ss.empty(),
                  "Must pass some spheres to have a bounding sphere");
#ifdef IMP_ALGEBRA_USE_IMP_CGAL
  return cgal::internal::get_enclosing_sphere(ss);
#else
  BoundingBox3D bb = get_bounding_box(ss[0]);
  for (unsigned int i = 1; i < ss.size(); ++i) {
    bb += get_bounding_box(ss[i]);
  }
  Vector3D c = .5 * (bb.get_corner(0) + bb.get_corner(1));
  double r = 0;
  for (unsigned int i = 0; i < ss.size(); ++i) {
    double d = (c - ss[i].get_center()).get_magnitude();
    d += ss[i].get_radius();
    r = std::max(r, d);
  }
  return Sphere3D(c, r);
#endif
}

ResultAlign2D get_rotational_alignment_no_preprocessing(const cv::Mat &POLAR1,
                                                  const cv::Mat &POLAR2) {
  IMP_LOG_TERSE(
    "starting 2D rotational alignment with no preprocessing" << std::endl);

  IMP_USAGE_CHECK(((POLAR1.rows==POLAR2.rows) && (POLAR1.cols==POLAR2.cols)),
    "get_rotational_alignment_no_preprocessing: Matrices have different size.");

  ResultAlign2D RA =get_translational_alignment_no_preprocessing(POLAR1,POLAR2);
  algebra::Vector2D shift=RA.first.get_translation();
  // The number of columns of the polar matrices
  // are the number of angles considered. Init a PolarResamplingParameters
  // to get the angle_step
  PolarResamplingParameters polar_params;
  polar_params.set_estimated_number_of_angles(POLAR1.cols);
  double angle =shift[0]*polar_params.get_angle_step();
  RA.first.set_rotation(angle);
  RA.first.set_translation(algebra::Vector2D(0.0,0.0));
  IMP_LOG_VERBOSE("Rotational Transformation= "
          << RA.first << " cross_correlation = " << RA.second << std::endl);
  return RA;
}

/* Apply the restraint to two atoms, two Scales, one experimental value.
 */
double
AmbiguousNOERestraint::unprotected_evaluate(DerivativeAccumulator *accum) const
{
  IMP_OBJECT_LOG;
  IMP_USAGE_CHECK(get_model(),
                  "You must at least register the restraint with the model"
                  << " before calling evaluate.");

  /* compute Icalc = 1/(gamma*d^6) where d = (sum d_i^-6)^(-1/6) */
  double vol = 0;
  Floats vols;
  IMP_CONTAINER_FOREACH(PairContainer, pc_,
                        {
            core::XYZ d0(get_model(), _1[0]);
            core::XYZ d1(get_model(), _1[1]);
            algebra::Vector3D c0 = d0.get_coordinates();
            algebra::Vector3D c1 = d1.get_coordinates();
            //will raise an error if c0 == c1
            double tmp = 1.0/(c0-c1).get_squared_magnitude();
            vols.push_back(IMP::cube(tmp)); // store di^-6
            vol += vols.back();
                        });

ProteinsAnchorsSamplingSpace read_protein_anchors_mapping(
    multifit::ProteomicsData *prots, const std::string &anchors_prot_map_fn,
    int max_paths) {
  ProteinsAnchorsSamplingSpace ret(prots);
  std::fstream in;
  IMP_LOG_TERSE("FN:" << anchors_prot_map_fn << std::endl);
  in.open(anchors_prot_map_fn.c_str(), std::fstream::in);
  if (!in.good()) {
    IMP_WARN(
        "Problem opening file " << anchors_prot_map_fn
                                << " for reading; returning empty mapping data"
                                << std::endl);
    in.close();
    return ret;
  }
  std::string line;
  // first line should be the anchors line
  getline(in, line);
  std::string anchors_fn =
      get_relative_path(anchors_prot_map_fn, parse_anchors_line(line));
  IMP_LOG_TERSE("FN:" << anchors_fn << std::endl);
  multifit::AnchorsData anchors_data =
      multifit::read_anchors_data(anchors_fn.c_str());
  ret.set_anchors(anchors_data);
  ret.set_anchors_filename(anchors_fn);
  while (!in.eof()) {
    if (!getline(in, line)) break;
    IMP_LOG_VERBOSE("working on line:" << line);
    IMP_USAGE_CHECK(is_protein_line(line), "the line should be a protein line");
    boost::tuple<std::string, std::string, IntsList> prot_data =
        parse_protein_line(anchors_prot_map_fn, line, max_paths);
    ret.set_paths_for_protein(boost::get<0>(prot_data),
                              boost::get<2>(prot_data));
    ret.set_paths_filename_for_protein(boost::get<0>(prot_data),
                                       boost::get<1>(prot_data));
  }
  return ret;
}

MonteCarloMoverResult NormalMover::do_propose() {
  IMP_OBJECT_LOG;
  boost::normal_distribution<double> mrng(0, stddev_);
  boost::variate_generator<RandomNumberGenerator &,
                           boost::normal_distribution<double> >
      sampler(random_number_generator, mrng);

  for (unsigned int i = 0; i < pis_.size(); ++i) {
    for (unsigned int j = 0; j < keys_.size(); ++j) {
      originals_[i][j] = get_model()->get_attribute(keys_[j], pis_[i]);
    }
    for (unsigned int j = 0; j < keys_.size(); ++j) {
      IMP_USAGE_CHECK(
          get_model()->get_is_optimized(keys_[j], pis_[i]),
          "NormalMover can't move non-optimized attribute. "
              << "particle: " << get_model()->get_particle_name(pis_[i])
              << "attribute: " << keys_[j]);
      get_model()->set_attribute(keys_[j], pis_[i],
                                 originals_[i][j] + sampler());
    }
  }
  return MonteCarloMoverResult(pis_, 1.0);
}

ResultAlign2D get_translational_alignment_no_preprocessing(const cv::Mat &M1,
                                                           const cv::Mat &M2) {
  IMP_LOG_TERSE("starting 2D translational alignment with no preprocessing"
                << std::endl);
  IMP_USAGE_CHECK(((M1.rows == M2.rows) && (M1.cols == M2.cols)),
                  "get_translational_alignment_no_preprocessing: "
                  "Matrices have different size.");

  cv::Mat corr;
  corr.create(M1.rows, M1.cols, CV_64FC1);
  get_correlation2d_no_preprocessing(M1, M2, corr);  // corr must be allocated!
  // Find the peak of the cross_correlation
  double max_cc;
  algebra::Vector2D peak = internal::get_peak(corr, &max_cc);

  // Convert the pixel with the maximum to a shift respect to the center
  algebra::Vector2D shift(peak[0] - static_cast<double>(corr.cols) / 2.,
                          peak[1] - static_cast<double>(corr.rows) / 2.);
  algebra::Transformation2D t(shift);
  IMP_LOG_VERBOSE(" Translational Transformation = "
                  << t << " cross_correlation = " << max_cc << std::endl);
  return ResultAlign2D(t, max_cc);
}

const RadiusDependentKernelParameters& KernelParameters::get_params(
  float radius,  float eps) {
  IMP_USAGE_CHECK(initialized_, "The Kernel Parameters are not initialized");
  typedef
    std::map<float, const RadiusDependentKernelParameters*>
    kernel_map;
  //we do not use find but use lower_bound and upper_bound to overcome
  //numerical instabilities


  //in maps, an iterator that addresses the location of an element
  //that with a key that is equal to or greater than the argument key,
  //or that addresses the location succeeding the last element in the
  //map if no match is found for the key.
  kernel_map::iterator lower_closest = radii2params_.lower_bound(radius);
  kernel_map::iterator upper_closest = radii2params_.upper_bound(radius);
   const RadiusDependentKernelParameters *closest = nullptr;
   if (algebra::get_are_almost_equal(radius,upper_closest->first,eps)) {
     closest = upper_closest->second;
     IMP_LOG_VERBOSE("for radius:"<<radius<<
             " the closest is:"<< upper_closest->first<<std::endl);
   }
   else {
     if (lower_closest != radii2params_.end()) {
       if (algebra::get_are_almost_equal(radius,lower_closest->first,eps)) {
         closest = lower_closest->second;
       }
     }
   }
   if (closest == nullptr) {
     IMP_WARN("could not find parameters for radius:"<<radius<<std::endl);
     IMP_WARN("Setting params for radius :"<<radius<<std::endl);
     return set_params(radius);
   } else {
     return *closest;
   }
}

ResultAlign2D get_translational_alignment(const cv::Mat &input,
                                          cv::Mat &m_to_align, bool apply) {
  IMP_LOG_TERSE("starting 2D translational alignment" << std::endl);
  IMP_USAGE_CHECK(
      (input.rows == m_to_align.rows) && (input.cols == m_to_align.cols),
      "em2d::align_translational: Matrices have different size.");
  cv::Mat corr;
  get_correlation2d(input, m_to_align, corr);
  double max_cc;
  algebra::Vector2D peak = internal::get_peak(corr, &max_cc);
  // Convert the pixel with the maximum to a shift respect to the center
  algebra::Vector2D shift(peak[0] - static_cast<double>(corr.cols) / 2.,
                          peak[1] - static_cast<double>(corr.rows) / 2.);
  algebra::Transformation2D t(shift);
  // Apply the shift if requested
  if (apply) {
    cv::Mat result;
    get_transformed(m_to_align, result, t);
    result.copyTo(m_to_align);
  }
  IMP_LOG_VERBOSE(" Transformation= " << t << " cross_correlation = " << max_cc
                                      << std::endl);
  return ResultAlign2D(t, max_cc);
}

//! Get the i-th weight
Float Weight::get_weight(int i) {
  IMP_USAGE_CHECK(i < get_number_of_states(), "Out of range");
  return get_particle()->get_value(get_weight_key(i));
}

double Simulator::do_simulate_wave(double time, double max_time_step_factor,
                                   double base) {
  IMP_FUNCTION_LOG;
  set_was_used(true);
  ParticleIndexes ps = get_simulation_particle_indexes();

  setup(ps);
  double target = current_time_ + time;
  IMP_USAGE_CHECK(max_time_step_factor > 1.0,
                  "simulate wave time step factor must be >1.0");

  // build wave into cur_ts
  double wave_max_ts = max_time_step_factor * max_time_step_;
  std::vector<double> ts_seq;
  {
    int n_half = 2;  // subwave length
    bool max_reached = false;
    double raw_wave_time = 0.0;
    do {
      double cur_ts = max_time_step_;
      // go up
      for (int i = 0; i < n_half; i++) {
        ts_seq.push_back(cur_ts);
        raw_wave_time += cur_ts;
        cur_ts *= base;
        if (cur_ts > wave_max_ts) {
          max_reached = true;
          break;
        }
      }
      // go down
      for (int i = 0; i < n_half; i++) {
        cur_ts /= base;
        ts_seq.push_back(cur_ts);
        raw_wave_time += cur_ts;
        if (cur_ts < max_time_step_) {
          break;
        }
      }
      n_half++;
    } while (!max_reached && raw_wave_time < time);
    // adjust to fit into time precisely
    unsigned int n = (unsigned int)std::ceil(time / raw_wave_time);
    double wave_time = time / n;
    double adjust = wave_time / raw_wave_time;
    IMP_LOG(PROGRESS, "Wave time step seq: ");
    for (unsigned int i = 0; i < ts_seq.size(); i++) {
      ts_seq[i] *= adjust;
      IMP_LOG(PROGRESS, ts_seq[i] << ", ");
    }
    IMP_LOG(PROGRESS, std::endl);
  }

  unsigned int i = 0;
  unsigned int k = ts_seq.size();  // n waves of k frames
  int orig_nf_left = (int)(time / max_time_step_);
  while (current_time_ < target) {
    last_time_step_ = do_step(ps, ts_seq[i++ % k]);
    current_time_ += last_time_step_;
    // emulate state updating by frames for the origin max_time_step
    // (for periodic optimizers)
    int nf_left = (int)((target - current_time_) / max_time_step_);
    while (orig_nf_left >= nf_left) {
      IMP_LOG(PROGRESS, "Updating states: " << orig_nf_left << "," << nf_left
                                            << " target time " << target
                                            << " current time " << current_time_
                                            << std::endl);
      update_states();  // needs to move
      orig_nf_left--;
    }
  }
  IMP_LOG(PROGRESS, "Simulated for " << i << " actual frames with waves of "
                                     << k << " frames each" << std::endl);
  IMP_USAGE_CHECK(current_time_ >= target - 0.001 * max_time_step_,
                  "simulations did not advance to target time for some reason");
  return Optimizer::get_scoring_function()->evaluate(false);
}