<commit_before>/*!
 *  Copyright (c) 2016 by Contributors
 * \file infer_shape.cc
 * \brief Inference the shapes given existin information.
 */
#include <nnvm/pass.h>
#include <nnvm/op_attr_types.h>
#include <nnvm/graph_attr_types.h>

namespace nnvm {
namespace pass {
namespace {

template<typename AttrType, typename IsNone, typename FDefault>
Graph InferAttr(Graph &&ret,
                const AttrType empty_val,
                const char* infer_name,
                const char* input_name,
                const char* attr_key_name,
                const char* attr_name,
                const char* unknown_name,
                IsNone fis_none,
                FDefault fdefault) {
  using AttrVector = std::vector<AttrType>;
  const IndexedGraph& idx = ret.indexed_graph();
  static auto& finfer_shape =
      Op::GetAttr<FInferNodeEntryAttr<AttrType> >(infer_name);
  static auto& backward_map =
      Op::GetAttr<FBackwardOutToInIndex>("FBackwardOutToInIndex");
  static auto& backward_in_grad =
      Op::GetAttr<FBackwardInGradIndex>("FBackwardInGradIndex");
  // reshape shape vector
  AttrVector rshape;
  if (ret.attrs.count(attr_name) != 0) {
    rshape = ret.MoveCopyAttr<AttrVector>(attr_name);
  } else {
    rshape.resize(idx.num_node_entries(), empty_val);
  }

  if (ret.attrs.count(input_name) != 0) {
    const AttrVector& shape_args = ret.GetAttr<AttrVector>(input_name);
    CHECK_LE(shape_args.size(), idx.input_nodes().size())
        << "More provided shapes than number of arguments.";
    for (size_t i = 0; i < shape_args.size(); ++i) {
      rshape[idx.entry_id(idx.input_nodes()[i], 0)] = shape_args[i];
    }
    // erase the provided arguments
    ret.attrs.erase(input_name);
  }

  std::string shape_attr_key;
  if (ret.attrs.count(attr_key_name) != 0) {
    shape_attr_key = ret.GetAttr<std::string>(attr_key_name);
    // erase the provided arguments
    ret.attrs.erase(attr_key_name);
  }
  // Temp space for shape inference.
  std::vector<AttrType> ishape, oshape;

  // inference step function for nid
  auto infer_step = [&](uint32_t nid) {
    const auto& inode = idx[nid];
    const uint32_t num_inputs = inode.inputs.size();
    const uint32_t num_outputs = inode.source->num_outputs();
    if (inode.source->is_variable()) {
      // Variable node. No operator. Only one output entry.
      CHECK(inode.source->op() == nullptr);
      CHECK_EQ(num_outputs, 1);
      const uint32_t out_ent_id = idx.entry_id(nid, 0);
      if (shape_attr_key.length() != 0 && fis_none(rshape[out_ent_id])) {
        auto it = inode.source->attrs.dict.find(shape_attr_key);
        if (it != inode.source->attrs.dict.end()) {
          std::istringstream is(it->second);
          CHECK(is >> rshape[out_ent_id]) << "Invalid attribute";
        }
      }
    } else if (backward_map.count(inode.source->op())) {
      // Backward operator inference.
      CHECK_GE(inode.control_deps.size(), 1)
        << "BackwardOp need to have control_deps to its forward op";
      const IndexedGraph::Node& fnode = idx[inode.control_deps[0]];
      // Inference the outputs of backward operator (equal to the inputs
      // of its corresponding forward operator).
      std::vector<uint32_t> out_map =
          backward_map[inode.source->op()](inode.source->attrs);
      for (size_t i = 0; i < out_map.size(); ++i) {
        uint32_t in_id = out_map[i];
        CHECK_LT(in_id, fnode.inputs.size());
        rshape[idx.entry_id(nid, i)] =
            rshape[idx.entry_id(fnode.inputs[in_id])];
      }
      if (backward_in_grad.count(inode.source->op())) {
        std::vector<uint32_t> in_grad =
          backward_in_grad[inode.source->op()](inode.source->attrs);
        CHECK_LE(in_grad.size(), fnode.source->num_outputs());
        for (size_t i = 0; i < in_grad.size(); ++i) {
          uint32_t eid = idx.entry_id(inode.inputs[in_grad[i]]);
          if (fis_none(rshape[eid])) {
            rshape[eid] = rshape[idx.entry_id(inode.control_deps[0], i)];
          }
        }
      }
    } else {
      bool forward_known = true;
      // Forward operator inference.
      ishape.resize(num_inputs, empty_val);
      for (uint32_t i = 0; i < ishape.size(); ++i) {
        ishape[i] = rshape[idx.entry_id(inode.inputs[i])];
        if (fis_none(ishape[i])) forward_known = false;
      }
      oshape.resize(num_outputs, empty_val);
      for (uint32_t i = 0; i < oshape.size(); ++i) {
        oshape[i] = rshape[idx.entry_id(nid, i)];
        if (fis_none(oshape[i])) forward_known = false;
      }
      if (!forward_known) {
        auto finfer = finfer_shape.get(inode.source->op(), fdefault);
        CHECK(finfer != nullptr)
          << "Attribute " << infer_name
          << " is not registed by op " << inode.source->op()->name;
        // Call inference function of the operator.
        forward_known = finfer(inode.source->attrs, &ishape, &oshape);
      }
      // Save to the result map.
      for (uint32_t i = 0; i < num_inputs; ++i) {
        rshape[idx.entry_id(inode.inputs[i])] = ishape[i];
      }
      for (uint32_t i = 0; i < num_outputs; ++i) {
        rshape[idx.entry_id(nid, i)] = oshape[i];
      }
    }
  };

  size_t num_unknown = 0;
  const int kMaxStep = 3;
  for (int i = 0; i < kMaxStep; ++i) {
    if (i % 2 == 0) {
      for (uint32_t nid = 0; nid < idx.num_nodes(); ++nid) {
        infer_step(nid);
      }
    } else {
      // backward inference
      for (uint32_t i = idx.num_nodes(); i != 0; --i) {
        infer_step(i - 1);
      }
    }
    num_unknown = 0;
    for (size_t i = 0; i < idx.num_node_entries(); ++i) {
      if (fis_none(rshape[i])) ++num_unknown;
    }
    if (num_unknown == 0) break;
  }
  // set the shapes
  ret.attrs[attr_name] = std::make_shared<any>(std::move(rshape));
  // number of nodes who knows the shape.
  ret.attrs[unknown_name] = std::make_shared<any>(num_unknown);
  return ret;
}

NNVM_REGISTER_PASS(InferShape)
.describe("Infer the shape of each node entries.")
.set_body([](Graph ret) {
    return InferAttr<TShape>(
        std::move(ret), TShape(),
        "FInferShape", "shape_inputs", "shape_attr_key",
        "shape", "shape_num_unknown_nodes",
        [](const TShape& s) { return s.ndim() == 0; },
        nullptr);
  })
.set_change_graph(false)
.provide_graph_attr("shape");

// inference fucntion for same type
inline bool SameType(const NodeAttrs& attrs,
                     std::vector<int> *iattr,
                     std::vector<int> *oattr) {
  int def_v = -1;
  for (int v : *oattr) {
    if (v != -1) {
      def_v = v; break;
    }
  }
  if (def_v == -1) {
    for (int v : *iattr) {
      if (v != -1) {
        def_v = v; break;
      }
    }
  }
  if (def_v == -1) return false;
  for (int& v : *oattr) {
    v = def_v;
  }
  for (int& v : *iattr) {
    v = def_v;
  }
  return true;
}

NNVM_REGISTER_PASS(InferType)
.describe("Infer the dtype of each node entries.")
.set_body([](Graph ret) {
    return InferAttr<int>(
        std::move(ret), -1,
        "FInferType", "dtype_inputs", "dtype_attr_key",
        "dtype", "dtype_num_unknown_nodes",
        [](const int t) { return t == -1; },
        SameType);
  })
.set_change_graph(false)
.provide_graph_attr("dtype");

DMLC_JSON_ENABLE_ANY(ShapeVector, list_shape);
DMLC_JSON_ENABLE_ANY(DTypeVector, list_int);
DMLC_JSON_ENABLE_ANY(size_t, size_t);

}  // namespace
}  // namespace pass
}  // namespace nnvm
<commit_msg>error info (#69)<commit_after>/*!
 *  Copyright (c) 2016 by Contributors
 * \file infer_shape.cc
 * \brief Inference the shapes given existin information.
 */
#include <nnvm/pass.h>
#include <nnvm/op_attr_types.h>
#include <nnvm/graph_attr_types.h>

namespace nnvm {
namespace pass {
namespace {

template<typename AttrType, typename IsNone, typename FDefault>
Graph InferAttr(Graph &&ret,
                const AttrType empty_val,
                const char* infer_name,
                const char* input_name,
                const char* attr_key_name,
                const char* attr_name,
                const char* unknown_name,
                IsNone fis_none,
                FDefault fdefault) {
  using AttrVector = std::vector<AttrType>;
  const IndexedGraph& idx = ret.indexed_graph();
  static auto& finfer_shape =
      Op::GetAttr<FInferNodeEntryAttr<AttrType> >(infer_name);
  static auto& backward_map =
      Op::GetAttr<FBackwardOutToInIndex>("FBackwardOutToInIndex");
  static auto& backward_in_grad =
      Op::GetAttr<FBackwardInGradIndex>("FBackwardInGradIndex");
  // reshape shape vector
  AttrVector rshape;
  if (ret.attrs.count(attr_name) != 0) {
    rshape = ret.MoveCopyAttr<AttrVector>(attr_name);
  } else {
    rshape.resize(idx.num_node_entries(), empty_val);
  }

  if (ret.attrs.count(input_name) != 0) {
    const AttrVector& shape_args = ret.GetAttr<AttrVector>(input_name);
    CHECK_LE(shape_args.size(), idx.input_nodes().size())
        << "More provided shapes than number of arguments.";
    for (size_t i = 0; i < shape_args.size(); ++i) {
      rshape[idx.entry_id(idx.input_nodes()[i], 0)] = shape_args[i];
    }
    // erase the provided arguments
    ret.attrs.erase(input_name);
  }

  std::string shape_attr_key;
  if (ret.attrs.count(attr_key_name) != 0) {
    shape_attr_key = ret.GetAttr<std::string>(attr_key_name);
    // erase the provided arguments
    ret.attrs.erase(attr_key_name);
  }
  // Temp space for shape inference.
  std::vector<AttrType> ishape, oshape;

  // inference step function for nid
  auto infer_step = [&](uint32_t nid) {
    const auto& inode = idx[nid];
    const uint32_t num_inputs = inode.inputs.size();
    const uint32_t num_outputs = inode.source->num_outputs();
    if (inode.source->is_variable()) {
      // Variable node. No operator. Only one output entry.
      CHECK(inode.source->op() == nullptr);
      CHECK_EQ(num_outputs, 1);
      const uint32_t out_ent_id = idx.entry_id(nid, 0);
      if (shape_attr_key.length() != 0 && fis_none(rshape[out_ent_id])) {
        auto it = inode.source->attrs.dict.find(shape_attr_key);
        if (it != inode.source->attrs.dict.end()) {
          std::istringstream is(it->second);
          CHECK(is >> rshape[out_ent_id]) << "Invalid attribute";
        }
      }
    } else if (backward_map.count(inode.source->op())) {
      // Backward operator inference.
      CHECK_GE(inode.control_deps.size(), 1)
        << "BackwardOp need to have control_deps to its forward op";
      const IndexedGraph::Node& fnode = idx[inode.control_deps[0]];
      // Inference the outputs of backward operator (equal to the inputs
      // of its corresponding forward operator).
      std::vector<uint32_t> out_map =
          backward_map[inode.source->op()](inode.source->attrs);
      for (size_t i = 0; i < out_map.size(); ++i) {
        uint32_t in_id = out_map[i];
        CHECK_LT(in_id, fnode.inputs.size());
        rshape[idx.entry_id(nid, i)] =
            rshape[idx.entry_id(fnode.inputs[in_id])];
      }
      if (backward_in_grad.count(inode.source->op())) {
        std::vector<uint32_t> in_grad =
          backward_in_grad[inode.source->op()](inode.source->attrs);
        CHECK_LE(in_grad.size(), fnode.source->num_outputs());
        for (size_t i = 0; i < in_grad.size(); ++i) {
          uint32_t eid = idx.entry_id(inode.inputs[in_grad[i]]);
          if (fis_none(rshape[eid])) {
            rshape[eid] = rshape[idx.entry_id(inode.control_deps[0], i)];
          }
        }
      }
    } else {
      bool forward_known = true;
      // Forward operator inference.
      ishape.resize(num_inputs, empty_val);
      for (uint32_t i = 0; i < ishape.size(); ++i) {
        ishape[i] = rshape[idx.entry_id(inode.inputs[i])];
        if (fis_none(ishape[i])) forward_known = false;
      }
      oshape.resize(num_outputs, empty_val);
      for (uint32_t i = 0; i < oshape.size(); ++i) {
        oshape[i] = rshape[idx.entry_id(nid, i)];
        if (fis_none(oshape[i])) forward_known = false;
      }
      if (!forward_known) {
        auto finfer = finfer_shape.get(inode.source->op(), fdefault);
        CHECK(finfer != nullptr)
          << "Attribute " << infer_name
          << " is not registed by op " << inode.source->op()->name;
        // Call inference function of the operator.
        try {
          forward_known = finfer(inode.source->attrs, &ishape, &oshape);
        } catch (const std::exception& e) {
          throw dmlc::Error(e.what() + std::string(" with ") + inode.source->attrs.name);
        }
      }
      // Save to the result map.
      for (uint32_t i = 0; i < num_inputs; ++i) {
        rshape[idx.entry_id(inode.inputs[i])] = ishape[i];
      }
      for (uint32_t i = 0; i < num_outputs; ++i) {
        rshape[idx.entry_id(nid, i)] = oshape[i];
      }
    }
  };

  size_t num_unknown = 0;
  const int kMaxStep = 3;
  for (int i = 0; i < kMaxStep; ++i) {
    if (i % 2 == 0) {
      for (uint32_t nid = 0; nid < idx.num_nodes(); ++nid) {
        infer_step(nid);
      }
    } else {
      // backward inference
      for (uint32_t i = idx.num_nodes(); i != 0; --i) {
        infer_step(i - 1);
      }
    }
    num_unknown = 0;
    for (size_t i = 0; i < idx.num_node_entries(); ++i) {
      if (fis_none(rshape[i])) ++num_unknown;
    }
    if (num_unknown == 0) break;
  }
  // set the shapes
  ret.attrs[attr_name] = std::make_shared<any>(std::move(rshape));
  // number of nodes who knows the shape.
  ret.attrs[unknown_name] = std::make_shared<any>(num_unknown);
  return ret;
}

NNVM_REGISTER_PASS(InferShape)
.describe("Infer the shape of each node entries.")
.set_body([](Graph ret) {
    return InferAttr<TShape>(
        std::move(ret), TShape(),
        "FInferShape", "shape_inputs", "shape_attr_key",
        "shape", "shape_num_unknown_nodes",
        [](const TShape& s) { return s.ndim() == 0; },
        nullptr);
  })
.set_change_graph(false)
.provide_graph_attr("shape");

// inference fucntion for same type
inline bool SameType(const NodeAttrs& attrs,
                     std::vector<int> *iattr,
                     std::vector<int> *oattr) {
  int def_v = -1;
  for (int v : *oattr) {
    if (v != -1) {
      def_v = v; break;
    }
  }
  if (def_v == -1) {
    for (int v : *iattr) {
      if (v != -1) {
        def_v = v; break;
      }
    }
  }
  if (def_v == -1) return false;
  for (int& v : *oattr) {
    v = def_v;
  }
  for (int& v : *iattr) {
    v = def_v;
  }
  return true;
}

NNVM_REGISTER_PASS(InferType)
.describe("Infer the dtype of each node entries.")
.set_body([](Graph ret) {
    return InferAttr<int>(
        std::move(ret), -1,
        "FInferType", "dtype_inputs", "dtype_attr_key",
        "dtype", "dtype_num_unknown_nodes",
        [](const int t) { return t == -1; },
        SameType);
  })
.set_change_graph(false)
.provide_graph_attr("dtype");

DMLC_JSON_ENABLE_ANY(ShapeVector, list_shape);
DMLC_JSON_ENABLE_ANY(DTypeVector, list_int);
DMLC_JSON_ENABLE_ANY(size_t, size_t);

}  // namespace
}  // namespace pass
}  // namespace nnvm
<|endoftext|>


<commit_before>// Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include "hpp/core/path-projector/global.hh"

#include <hpp/util/debug.hh>
#include <hpp/model/configuration.hh>

#include <hpp/core/path-vector.hh>
#include <hpp/core/interpolated-path.hh>
#include <hpp/core/config-projector.hh>

#include <limits>
#include <queue>
#include <stack>

namespace hpp {
  namespace core {
    namespace pathProjector {
      Global::Global (const DistancePtr_t& distance,
				const SteeringMethodPtr_t& steeringMethod,
				value_type step) :
        PathProjector (distance, steeringMethod), step_ (step),
        alphaMin (0.2), alphaMax (0.95)
      {}

      bool Global::impl_apply (const PathPtr_t& path,
				    PathPtr_t& proj) const
      {
	assert (path);
	bool success = false;
	PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, path);
	if (!pv) {
          if (!path->constraints()
              || !path->constraints()->configProjector()) {
            proj = path;
            success = true;
          } else {
            success = project (path, proj);
          }
	} else {
	  PathVectorPtr_t res = PathVector::create
	    (pv->outputSize (), pv->outputDerivativeSize ());
	  PathPtr_t part;
	  success = true;
	  for (size_t i = 0; i < pv->numberPaths (); i++) {
	    if (!apply (pv->pathAtRank (i), part)) {
	      // We add the path only if part is not NULL and:
	      // - either its length is not zero,
	      // - or it's not the first one.
	      if (part && (part->length () > 0 || i == 0)) {
		res->appendPath (part);
	      }
	      success = false;
	      break;
	    }
	    res->appendPath (part);
	  }
	  proj = res;
	}
	assert (proj);
	assert ((proj->initial () - path->initial ()).isZero());
	assert (!success || (proj->end () - path->end ()).isZero());
	return success;
      }

      bool Global::project (const PathPtr_t& path, PathPtr_t& proj) const
      {
        Configs_t cfgs;
        ConfigProjector& p = *path->constraints ()->configProjector ();
        initialConfigList (path, cfgs);
        if (cfgs.size() == 2) { // Shorter than step_
          proj = path;
          return true;
        }
        assert ((cfgs.back () - path->end ()).isZero ());

        Bools_t projected (cfgs.size () - 2, false);
        Alphas_t alphas   (cfgs.size () - 2, alphaMin);
        Lengths_t lengths (cfgs.size () - 1, 0);
        Configs_t::iterator last = --(cfgs.end());

        std::size_t nbIter = 0;
        const std::size_t maxIter = p.maxIterations ();
        const std::size_t maxCfgNum =
          2 + (std::size_t)(10 * path->length() / step_);

        hppDout (info, "start with " << cfgs.size () << " configs");
        while (!projectOneStep (p, cfgs, last, projected, lengths, alphas)) {
          assert ((cfgs.back () - path->end ()).isZero ());
          if (cfgs.size() < maxCfgNum) {
            const size_type newCs =
              reinterpolate (p.robot(), cfgs, last, projected, lengths, alphas,
                  step_);
            if (newCs > 0) {
              nbIter = 0;
              hppDout (info, "Added " << newCs << " configs. Cur / Max = "
                  << cfgs.size() << '/' << maxCfgNum);
            } else if (newCs < 0) {
              hppDout (info, "Removed " << -newCs << " configs. Cur / Max = "
                  << cfgs.size() << '/' << maxCfgNum);
            }
          }

          nbIter++;

          if (nbIter > maxIter) break;
        }

        // Build the projection
        return createPath (p.robot(), path->constraints(),
            cfgs, projected, lengths, proj);
      }

      bool Global::projectOneStep (ConfigProjector& p,
          Configs_t& q, Configs_t::iterator& last,
          Bools_t& b, Lengths_t& l, Alphas_t& a) const
      {
        /// First and last should not be updated
        const Configs_t::iterator begin = ++(q.begin ());
        const Configs_t::iterator end   = --(q.end ());
        Bools_t  ::iterator itB   =   (b.begin ());
        Alphas_t ::iterator itA   =   (a.begin ());
        Lengths_t::iterator itL   =   (l.begin ());
        Configs_t::iterator itCp  =   (q.begin ());
        bool allAreSatisfied = true;
        bool curUpdated = false, prevUpdated = false;
        /// Eigen matrices storage order defaults to column major.
        Eigen::Matrix <value_type, Eigen::Dynamic, 2>
          oldQ (p.robot()->configSize(),2);
        Eigen::Matrix <value_type, Eigen::Dynamic, 2>
          dq (p.robot()->numberDof(),2);
        dq.setZero();
        vector_t qMinusQPrev (p.robot()->numberDof());
        size_type iCol = 0;
        size_type iNCol = (iCol+1)%2;
        for (Configs_t::iterator it = begin; it != last; ++it) {
          if (!*itB) {
            oldQ.col(iCol) = *it;
            *itB = p.oneStep (*it, dq.col(iCol),*itA);
            *itA = alphaMax - 0.8 * (alphaMax - *itA);
            allAreSatisfied = allAreSatisfied && *itB;
            curUpdated = true;
            if (prevUpdated) {

              /// Detect large increase in size
              hpp::model::difference (p.robot(), oldQ.col(iCol), oldQ.col(iNCol), qMinusQPrev);
              const vector_t deltaDQ = dq.col(iCol) - dq.col(iNCol);
              const value_type N2 = qMinusQPrev.squaredNorm();
              if (sqrt(N2) < Eigen::NumTraits<value_type>::dummy_precision ()) {
                hppDout (error, "The two config should be removed:"
                    << "\noldQ = " << oldQ.transpose()
                    << "\nqMinusQPrev = " << qMinusQPrev.transpose()
                    << "\nDistance is " << d (oldQ.col(iCol), oldQ.col(iNCol))
                    << "\nand in mem  " << *itL
                    );
              } else {
                const value_type alphaSquare = 1 - 2 * qMinusQPrev.dot(deltaDQ) / N2 + qMinusQPrev.squaredNorm() / N2;
                // alpha > 4 => the distance between the two points has been
                // mutiplied by more than 4.
                if (alphaSquare > 16) {
                  hppDout (error, "alpha^2 = " << alphaSquare
                      << "\nqMinusQPrev = " << qMinusQPrev.transpose()
                      << "\ndeltaDQ = " << deltaDQ.transpose());
                  last = it; --last;
                  return false;
                }
              }

              const value_type limitCos = 0.5;
              vector_t dots = dq.colwise().normalized ().transpose() * qMinusQPrev.normalized();
              // Check if both updates are pointing outward
              if (dots[iCol] > limitCos && dots[iNCol] < - limitCos) {
                hppDout (error, "Descent step is going in opposite direction: "
                    << dots << ". It is likely a discontinuity.");
                  last = it; --last;
                  return false;
              }
              iNCol = iCol;
              iCol = (iCol+1)%2;
            }
          }
          if (prevUpdated || curUpdated)
            *itL = d (*itCp, *it);
          prevUpdated = curUpdated;
          curUpdated = false;
          ++itCp;
          ++itB;
          ++itL;
        }
        if (prevUpdated)
          *itL = d (*itCp, *end);
        return allAreSatisfied;
      }

      size_type Global::reinterpolate (const DevicePtr_t& robot,
          Configs_t& q, const Configs_t::iterator& last,
          Bools_t& b, Lengths_t& l, Alphas_t& a,
          const value_type& maxDist) const
      {
        Configs_t::iterator begin = ++(q.begin ());
        Configs_t::iterator end   = last; ++end;
        Bools_t  ::iterator itB   =   (b.begin ());
        Alphas_t ::iterator itA   =   (a.begin ());
        Lengths_t::iterator itL   =   (l.begin ());
        Configs_t::iterator itCp  =   (q.begin ());
        Configuration_t newQ (robot->configSize ());
        size_type nbNewC = 0;
        for (Configs_t::iterator it = begin; it != last; ++it) {
          if (*itL > maxDist) {
            ++nbNewC;
            hpp::model::interpolate (robot, *itCp, *it, 0.5, newQ);
            // FIXME: make sure the iterator are valid after insertion
            // Insert new respective elements
            it  = q.insert (it, newQ);
            itB = b.insert (itB, false);
            itA = a.insert (itA, alphaMin);
            itL = l.insert (itL, d (*itCp, *it));
            // Update length after
            Configs_t::iterator itNC = it;  ++itNC;
            Lengths_t::iterator itNL = itL; ++itNL;
            *itNL = d (*it, *itNC);
            // FIXME: End has changed ?
            end = q.end();
            it = itCp;
            continue;
          } else if (*itL < maxDist * 1e-2) {
            nbNewC--;
            hppDout (warning, "Removing configuration: " << it->transpose()
                << "\nToo close to: " << itCp->transpose()
                );
            // The distance to the previous point is very small.
            // This point can safely be removed.
            it  = q.erase (it);
            itB = b.erase (itB);
            itA = a.erase (itA);
            itL = l.erase (itL);
            // Update length
            *itL = d (*itCp, *it);
            it = itCp;
            continue;
          }
          ++itCp;
          ++itB;
          ++itA;
          ++itL;
        }
        return nbNewC;
      }

      bool Global::createPath (const DevicePtr_t& robot,
          const ConstraintSetPtr_t& constraint,
          const Configs_t& q, const Configs_t::iterator&,
          const Bools_t& b, const Lengths_t& l, PathPtr_t& result) const
      {
        /// Compute total length
        value_type length = 0;
        Lengths_t::const_iterator itL   = (l.begin ());
        Bools_t  ::const_iterator itB   = (b.begin ());
        Configs_t::const_iterator itCl  = (q.begin ());
        bool fullyProjected = true;
        for (; itB != b.end (); ++itB) {
          if (!*itB || *itL > step_) {
            fullyProjected = false;
            break;
          }
          length += *itL;
          ++itCl;
          ++itL;
        }
        if (fullyProjected) {
          length += *itL;
          ++itCl;
          ++itL;
          assert (itL == l.end ());
        }

        InterpolatedPathPtr_t out = InterpolatedPath::create
          (robot, q.front(), *itCl, length, constraint);

        if (itCl != q.begin ()) {
          length = 0;
          Lengths_t::const_iterator itL   =   (l.begin ());
          Configs_t::const_iterator begin = ++(q.begin ());
          for (Configs_t::const_iterator it = begin; it != itCl; ++it) {
            length += *itL;
            out->insert (length, *it);
            ++itL;
          }
        } else {
          hppDout (info, "Path of length 0");
          assert (!fullyProjected);
        }
        result = out;
        hppDout (info, "Projection succeeded ? " << fullyProjected);
        return fullyProjected;
      }

      void Global::initialConfigList (const PathPtr_t& path,
          Configs_t& cfgs) const
      {
        InterpolatedPathPtr_t ip =
          HPP_DYNAMIC_PTR_CAST (InterpolatedPath, path);
        if (ip) {
          // Get the waypoint of ip
          const InterpolatedPath::InterpolationPoints_t& ips =
            ip->interpolationPoints();
          for (InterpolatedPath::InterpolationPoints_t::const_iterator it =
              ips.begin (); it != ips.end (); ++it) {
            cfgs.push_back (it->second);
          }
        } else {
          const value_type L = path->length ();
          Configuration_t q (path->outputSize ());
          cfgs.push_back (path->initial ());
          // Factor 0.99 is to ensure that the distance between two consecutives
          // configurations will be smaller that step_
          for (value_type t = step_; t < L; t += step_*0.99) {
            // Interpolate without taking care of the constraints
            // FIXME: Path must not be a PathVector otherwise the constraints
            // are applied.
            path->at (t, q);
            cfgs.push_back (q);
          }
          cfgs.push_back (path->end ());
        }
      }
    } // namespace pathProjector
  } // namespace core
} // namespace hpp
<commit_msg>Add banchmark output to pathProjector::Global<commit_after>// Copyright (c) 2015, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#include "hpp/core/path-projector/global.hh"

#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>
#include <hpp/model/configuration.hh>

#include <hpp/core/path-vector.hh>
#include <hpp/core/interpolated-path.hh>
#include <hpp/core/config-projector.hh>

#include <limits>
#include <queue>
#include <stack>

namespace hpp {
  namespace core {
    namespace pathProjector {
      namespace {
        HPP_DEFINE_TIMECOUNTER (globalPathProjector_initCfgList);
        HPP_DEFINE_TIMECOUNTER (globalPathProjector_projOneStep);
        HPP_DEFINE_TIMECOUNTER (globalPathProjector_reinterpolate);
        HPP_DEFINE_TIMECOUNTER (globalPathProjector_createPath);
      }
      Global::Global (const DistancePtr_t& distance,
				const SteeringMethodPtr_t& steeringMethod,
				value_type step) :
        PathProjector (distance, steeringMethod), step_ (step),
        alphaMin (0.2), alphaMax (0.95)
      {}

      bool Global::impl_apply (const PathPtr_t& path,
				    PathPtr_t& proj) const
      {
	assert (path);
	bool success = false;
	PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, path);
	if (!pv) {
          if (!path->constraints()
              || !path->constraints()->configProjector()) {
            proj = path;
            success = true;
          } else {
            success = project (path, proj);
          }
	} else {
	  PathVectorPtr_t res = PathVector::create
	    (pv->outputSize (), pv->outputDerivativeSize ());
	  PathPtr_t part;
	  success = true;
	  for (size_t i = 0; i < pv->numberPaths (); i++) {
	    if (!apply (pv->pathAtRank (i), part)) {
	      // We add the path only if part is not NULL and:
	      // - either its length is not zero,
	      // - or it's not the first one.
	      if (part && (part->length () > 0 || i == 0)) {
		res->appendPath (part);
	      }
	      success = false;
	      break;
	    }
	    res->appendPath (part);
	  }
	  proj = res;
	}
	assert (proj);
	assert ((proj->initial () - path->initial ()).isZero());
	assert (!success || (proj->end () - path->end ()).isZero());
	return success;
      }

      bool Global::project (const PathPtr_t& path, PathPtr_t& proj) const
      {
        Configs_t cfgs;
        ConfigProjector& p = *path->constraints ()->configProjector ();
        HPP_START_TIMECOUNTER(globalPathProjector_initCfgList);
        initialConfigList (path, cfgs);
        if (cfgs.size() == 2) { // Shorter than step_
          proj = path;
          return true;
        }
        assert ((cfgs.back () - path->end ()).isZero ());
        HPP_STOP_AND_DISPLAY_TIMECOUNTER(globalPathProjector_initCfgList);

        Bools_t projected (cfgs.size () - 2, false);
        Alphas_t alphas   (cfgs.size () - 2, alphaMin);
        Lengths_t lengths (cfgs.size () - 1, 0);
        Configs_t::iterator last = --(cfgs.end());

        std::size_t nbIter = 0;
        const std::size_t maxIter = p.maxIterations ();
        const std::size_t maxCfgNum =
          2 + (std::size_t)(10 * path->length() / step_);

        hppDout (info, "start with " << cfgs.size () << " configs");
        while (!projectOneStep (p, cfgs, last, projected, lengths, alphas)) {
          assert ((cfgs.back () - path->end ()).isZero ());
          if (cfgs.size() < maxCfgNum) {
            const size_type newCs =
              reinterpolate (p.robot(), cfgs, last, projected, lengths, alphas,
                  step_);
            if (newCs > 0) {
              nbIter = 0;
              hppDout (info, "Added " << newCs << " configs. Cur / Max = "
                  << cfgs.size() << '/' << maxCfgNum);
            } else if (newCs < 0) {
              hppDout (info, "Removed " << -newCs << " configs. Cur / Max = "
                  << cfgs.size() << '/' << maxCfgNum);
            }
          }

          nbIter++;

          if (nbIter > maxIter) break;
        }
        HPP_DISPLAY_TIMECOUNTER(globalPathProjector_projOneStep);
        HPP_DISPLAY_TIMECOUNTER(globalPathProjector_reinterpolate);

        // Build the projection
        HPP_START_TIMECOUNTER (globalPathProjector_createPath);
        bool ret = createPath (p.robot(), path->constraints(),
            cfgs, last, projected, lengths, proj);
        HPP_STOP_AND_DISPLAY_TIMECOUNTER (globalPathProjector_createPath);
        return ret;
      }

      bool Global::projectOneStep (ConfigProjector& p,
          Configs_t& q, Configs_t::iterator& last,
          Bools_t& b, Lengths_t& l, Alphas_t& a) const
      {
        HPP_START_TIMECOUNTER(globalPathProjector_projOneStep);
        /// First and last should not be updated
        const Configs_t::iterator begin = ++(q.begin ());
        const Configs_t::iterator end   = --(q.end ());
        Bools_t  ::iterator itB   =   (b.begin ());
        Alphas_t ::iterator itA   =   (a.begin ());
        Lengths_t::iterator itL   =   (l.begin ());
        Configs_t::iterator itCp  =   (q.begin ());
        bool allAreSatisfied = true;
        bool curUpdated = false, prevUpdated = false;
        /// Eigen matrices storage order defaults to column major.
        Eigen::Matrix <value_type, Eigen::Dynamic, 2>
          oldQ (p.robot()->configSize(),2);
        Eigen::Matrix <value_type, Eigen::Dynamic, 2>
          dq (p.robot()->numberDof(),2);
        dq.setZero();
        vector_t qMinusQPrev (p.robot()->numberDof());
        size_type iCol = 0;
        size_type iNCol = (iCol+1)%2;
        for (Configs_t::iterator it = begin; it != last; ++it) {
          if (!*itB) {
            oldQ.col(iCol) = *it;
            *itB = p.oneStep (*it, dq.col(iCol),*itA);
            *itA = alphaMax - 0.8 * (alphaMax - *itA);
            allAreSatisfied = allAreSatisfied && *itB;
            curUpdated = true;
            if (prevUpdated) {

              /// Detect large increase in size
              hpp::model::difference (p.robot(), oldQ.col(iCol), oldQ.col(iNCol), qMinusQPrev);
              const vector_t deltaDQ = dq.col(iCol) - dq.col(iNCol);
              const value_type N2 = qMinusQPrev.squaredNorm();
              if (sqrt(N2) < Eigen::NumTraits<value_type>::dummy_precision ()) {
                hppDout (error, "The two config should be removed:"
                    << "\noldQ = " << oldQ.transpose()
                    << "\nqMinusQPrev = " << qMinusQPrev.transpose()
                    << "\nDistance is " << d (oldQ.col(iCol), oldQ.col(iNCol))
                    << "\nand in mem  " << *itL
                    );
              } else {
                const value_type alphaSquare = 1 - 2 * qMinusQPrev.dot(deltaDQ) / N2 + qMinusQPrev.squaredNorm() / N2;
                // alpha > 4 => the distance between the two points has been
                // mutiplied by more than 4.
                if (alphaSquare > 16) {
                  hppDout (error, "alpha^2 = " << alphaSquare
                      << "\nqMinusQPrev = " << qMinusQPrev.transpose()
                      << "\ndeltaDQ = " << deltaDQ.transpose());
                  last = it; --last;
                  return false;
                }
              }

              const value_type limitCos = 0.5;
              vector_t dots = dq.colwise().normalized ().transpose() * qMinusQPrev.normalized();
              // Check if both updates are pointing outward
              if (dots[iCol] > limitCos && dots[iNCol] < - limitCos) {
                hppDout (error, "Descent step is going in opposite direction: "
                    << dots << ". It is likely a discontinuity.");
                  last = it; --last;
                  return false;
              }
              iNCol = iCol;
              iCol = (iCol+1)%2;
            }
          }
          if (prevUpdated || curUpdated)
            *itL = d (*itCp, *it);
          prevUpdated = curUpdated;
          curUpdated = false;
          ++itCp;
          ++itB;
          ++itL;
        }
        if (prevUpdated)
          *itL = d (*itCp, *end);
        HPP_STOP_TIMECOUNTER(globalPathProjector_projOneStep);
        return allAreSatisfied;
      }

      size_type Global::reinterpolate (const DevicePtr_t& robot,
          Configs_t& q, const Configs_t::iterator& last,
          Bools_t& b, Lengths_t& l, Alphas_t& a,
          const value_type& maxDist) const
      {
        HPP_START_TIMECOUNTER(globalPathProjector_reinterpolate);
        Configs_t::iterator begin = ++(q.begin ());
        Configs_t::iterator end   = last; ++end;
        Bools_t  ::iterator itB   =   (b.begin ());
        Alphas_t ::iterator itA   =   (a.begin ());
        Lengths_t::iterator itL   =   (l.begin ());
        Configs_t::iterator itCp  =   (q.begin ());
        Configuration_t newQ (robot->configSize ());
        size_type nbNewC = 0;
        for (Configs_t::iterator it = begin; it != last; ++it) {
          if (*itL > maxDist) {
            ++nbNewC;
            hpp::model::interpolate (robot, *itCp, *it, 0.5, newQ);
            // FIXME: make sure the iterator are valid after insertion
            // Insert new respective elements
            it  = q.insert (it, newQ);
            itB = b.insert (itB, false);
            itA = a.insert (itA, alphaMin);
            itL = l.insert (itL, d (*itCp, *it));
            // Update length after
            Configs_t::iterator itNC = it;  ++itNC;
            Lengths_t::iterator itNL = itL; ++itNL;
            *itNL = d (*it, *itNC);
            // FIXME: End has changed ?
            end = q.end();
            it = itCp;
            continue;
          } else if (*itL < maxDist * 1e-2) {
            nbNewC--;
            hppDout (warning, "Removing configuration: " << it->transpose()
                << "\nToo close to: " << itCp->transpose()
                );
            // The distance to the previous point is very small.
            // This point can safely be removed.
            it  = q.erase (it);
            itB = b.erase (itB);
            itA = a.erase (itA);
            itL = l.erase (itL);
            // Update length
            *itL = d (*itCp, *it);
            it = itCp;
            continue;
          }
          ++itCp;
          ++itB;
          ++itA;
          ++itL;
        }
        HPP_STOP_TIMECOUNTER(globalPathProjector_reinterpolate);
        return nbNewC;
      }

      bool Global::createPath (const DevicePtr_t& robot,
          const ConstraintSetPtr_t& constraint,
          const Configs_t& q, const Configs_t::iterator&,
          const Bools_t& b, const Lengths_t& l, PathPtr_t& result) const
      {
        /// Compute total length
        value_type length = 0;
        Lengths_t::const_iterator itL   = (l.begin ());
        Bools_t  ::const_iterator itB   = (b.begin ());
        Configs_t::const_iterator itCl  = (q.begin ());
        bool fullyProjected = true;
        for (; itB != b.end (); ++itB) {
          if (!*itB || *itL > step_) {
            fullyProjected = false;
            break;
          }
          length += *itL;
          ++itCl;
          ++itL;
        }
        if (fullyProjected) {
          length += *itL;
          ++itCl;
          ++itL;
          assert (itL == l.end ());
        }

        InterpolatedPathPtr_t out = InterpolatedPath::create
          (robot, q.front(), *itCl, length, constraint);

        if (itCl != q.begin ()) {
          length = 0;
          Lengths_t::const_iterator itL   =   (l.begin ());
          Configs_t::const_iterator begin = ++(q.begin ());
          for (Configs_t::const_iterator it = begin; it != itCl; ++it) {
            length += *itL;
            out->insert (length, *it);
            ++itL;
          }
        } else {
          hppDout (info, "Path of length 0");
          assert (!fullyProjected);
        }
        result = out;
        hppDout (info, "Projection succeeded ? " << fullyProjected);
        return fullyProjected;
      }

      void Global::initialConfigList (const PathPtr_t& path,
          Configs_t& cfgs) const
      {
        InterpolatedPathPtr_t ip =
          HPP_DYNAMIC_PTR_CAST (InterpolatedPath, path);
        if (ip) {
          // Get the waypoint of ip
          const InterpolatedPath::InterpolationPoints_t& ips =
            ip->interpolationPoints();
          for (InterpolatedPath::InterpolationPoints_t::const_iterator it =
              ips.begin (); it != ips.end (); ++it) {
            cfgs.push_back (it->second);
          }
        } else {
          const value_type L = path->length ();
          Configuration_t q (path->outputSize ());
          cfgs.push_back (path->initial ());
          // Factor 0.99 is to ensure that the distance between two consecutives
          // configurations will be smaller that step_
          for (value_type t = step_; t < L; t += step_*0.99) {
            // Interpolate without taking care of the constraints
            // FIXME: Path must not be a PathVector otherwise the constraints
            // are applied.
            path->at (t, q);
            cfgs.push_back (q);
          }
          cfgs.push_back (path->end ());
        }
      }
    } // namespace pathProjector
  } // namespace core
} // namespace hpp
<|endoftext|>


<commit_before>/***************************Copyright-DO-NOT-REMOVE-THIS-LINE**
 * CONDOR Copyright Notice
 *
 * See LICENSE.TXT for additional notices and disclaimers.
 *
 * Copyright (c)1990-1998 CONDOR Team, Computer Sciences Department,
 * University of Wisconsin-Madison, Madison, WI.  All Rights Reserved.
 * No use of the CONDOR Software Program Source Code is authorized
 * without the express consent of the CONDOR Team.  For more information
 * contact: CONDOR Team, Attention: Professor Miron Livny,
 * 7367 Computer Sciences, 1210 W. Dayton St., Madison, WI 53706-1685,
 * (608) 262-0856 or miron@cs.wisc.edu.
 *
 * U.S. Government Rights Restrictions: Use, duplication, or disclosure
 * by the U.S. Government is subject to restrictions as set forth in
 * subparagraph (c)(1)(ii) of The Rights in Technical Data and Computer
 * Software clause at DFARS 252.227-7013 or subparagraphs (c)(1) and
 * (2) of Commercial Computer Software-Restricted Rights at 48 CFR
 * 52.227-19, as applicable, CONDOR Team, Attention: Professor Miron
 * Livny, 7367 Computer Sciences, 1210 W. Dayton St., Madison,
 * WI 53706-1685, (608) 262-0856 or miron@cs.wisc.edu.
****************************Copyright-DO-NOT-REMOVE-THIS-LINE**/

/***********************************************************************
*
* Print declarations from the condor config files, condor_config and
* condor_config.local.  Declarations in files specified by
* LOCAL_CONFIG_FILE override those in the global config file, so this
* prints the same declarations found by condor programs using the
* config() routine.
*
* In addition, the configuration of remote machines can be queried.
* You can specify either a name or sinful string you want to connect
* to, what kind of daemon you want to query, and what pool to query to
* find the specified daemon.
*
***********************************************************************/

#include "condor_common.h"
#include "condor_config.h"
#include "condor_io.h"
#include "condor_uid.h"
#include "match_prefix.h"
#include "string_list.h"
#include "get_daemon_addr.h"
#include "daemon_types.h"

char	*MyName;
char	*mySubSystem = NULL;
StringList params;
daemonType dt = DT_MASTER;

// The pure-tools (PureCoverage, Purify, etc) spit out a bunch of
// stuff to stderr, which is where we normally put our error
// messages.  To enable config_val.test to produce easily readable
// output, even with pure-tools, we just write everything to stdout.
#if defined( PURE_DEBUG )
#	define stderr stdout
#endif

enum PrintType {CONDOR_OWNER, CONDOR_TILDE, CONDOR_NONE};

// On some systems, the output from config_val sometimes doesn't show
// up unless we explicitly flush before we exit.
void
my_exit( int status )
{
	fflush( stdout );
	fflush( stderr );
	exit( status );
}

void
usage()
{
	fprintf( stderr, "Usage: %s [options] variable [variable] ...\n", MyName );
	fprintf( stderr, "   Valid options are:\n" );
	fprintf( stderr, "   -name daemon_name\t(query the specified daemon for its configuration)\n" );
	fprintf( stderr, "   -pool hostname\t(use the given central manager to find daemons)\n" );
	fprintf( stderr, "   -address <ip:port>\t(connect to the given ip/port)\n" );

	fprintf( stderr, "   -master\t\t(query the master [default])\n" );
	fprintf( stderr, "   -schedd\t\t(query the schedd)\n" );
	fprintf( stderr, "   -startd\t\t(query the startd)\n" );
	fprintf( stderr, "   -collector\t\t(query the collector)\n" );
	fprintf( stderr, "   -negotiator\t\t(query the negotiator)\n" );
	my_exit( 1 );
}

char* GetRemoteParam( char*, char*, char*, char* );

main( int argc, char* argv[] )
{
	char	*value, *tmp, *host = NULL;
	char	*addr = NULL, *name = NULL, *pool = NULL;
	int		i;

	PrintType pt = CONDOR_NONE;

	MyName = argv[0];

	for( i=1; i<argc; i++ ) {
		if( match_prefix( argv[i], "-host" ) ) {
			if( argv[i + 1] ) {
				host = strdup( argv[++i] );
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-name" ) ) {
			if( argv[i + 1] ) {
				i++;
				if( (tmp = get_daemon_name(argv[i])) ) {
					name = strdup( tmp );
				} else {
					fprintf( stderr, "%s: unknown host %s\n", MyName,
							 get_host_part(argv[i]) );
					my_exit( 1 );
				}
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-address" ) ) {
			if( argv[i + 1] ) {
				i++;
				if( is_valid_sinful(argv[i]) ) {
					addr = strdup( argv[i] );
				} else {
					fprintf( stderr, "%s: invalid address %s\n"
							 "Address must be of the form \"<111.222.333.444:555>\n"
							 "   where 111.222.333.444 is the ip address and"
							 "555 is the port\n  you wish to connect to (the"
							 "punctuation is important.\n", MyName, argv[i] );
					my_exit( 1 );
				}
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-pool" ) ) {
			if( argv[i + 1] ) {
				i++;
				if( (tmp = get_daemon_name(argv[i])) ) {
					pool = strdup( tmp );
				} else {
					fprintf( stderr, "%s: unknown host %s\n", MyName,
							 get_host_part(argv[i]) );
					my_exit( 1 );
				}
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-owner" ) ) {
			pt = CONDOR_OWNER;
		} else if( match_prefix( argv[i], "-tilde" ) ) {
			pt = CONDOR_TILDE;
		} else if( match_prefix( argv[i], "-master" ) ) {
			dt = DT_MASTER;
		} else if( match_prefix( argv[i], "-schedd" ) ) {
			dt = DT_SCHEDD;
		} else if( match_prefix( argv[i], "-startd" ) ) {
			dt = DT_STARTD;
		} else if( match_prefix( argv[i], "-collector" ) ) {
			dt = DT_COLLECTOR;
		} else if( match_prefix( argv[i], "-negotiator" ) ) {
			dt = DT_NEGOTIATOR;
		} else if( match_prefix( argv[i], "-" ) ) {
			usage();
		} else {
			params.append( strdup( argv[i] ) );
		}
	}

		// If we didn't get told what subsystem we should use, set it
		// to "TOOL".
	if( !mySubSystem ) {
		mySubSystem = strdup( "TOOL" );
	}

		// Want to do this before we try to find the address of a
		// remote daemon, since if there's no -pool option, we need to
		// param() for the COLLECTOR_HOST to contact.
	if( host ) {
		config_host( 0, host );
	} else {
		config( 0 );
	}

	if( name || pool ) {
		addr = get_daemon_addr( dt, name, pool );
		if( ! addr ) {
			fprintf( stderr, "Can't find address for %s %s\n",
					 daemon_string(dt), name );
			fprintf( stderr, "Perhaps you need to query another pool.\n" );
			my_exit( 1 );
		}
	}

	if( pt == CONDOR_TILDE ) {
		if( (tmp = get_tilde()) ) {
			printf( "%s\n", tmp );
			my_exit( 0 );
		} else {
			fprintf( stderr,
					 "Error: Specified -tilde but can't find %s\n",
					 "condor's home directory." );
			my_exit( 1 );
		}
	}

	if( pt == CONDOR_OWNER ) {
		printf( "%s\n", get_condor_username() );
		my_exit( 0 );
	}

	params.rewind();

	if( ! params.number() ) {
		usage();
	}

	while( (tmp = params.next()) ) {
		if( name || pool || addr ) {
			value = GetRemoteParam( name, addr, pool, tmp );
		} else {
			value = param( tmp );
		}
		if( value == NULL ) {
			fprintf(stderr, "Not defined: %s\n", tmp);
			my_exit( 1 );
		} else {
			printf("%s\n", value);
			free( value );
		}
	}
	my_exit( 0 );
}

char*
GetRemoteParam( char* name, char* addr, char* pool, char* param_name )
{
	ReliSock s;
	s.timeout( 30 );
	char	*val = NULL;
	int 	cmd = CONFIG_VAL;

	if( !name ) {
		name = "";
	}

	if( ! s.connect( addr, 0 ) ) {
		fprintf( stderr, "Can't connect to %s on %s %s\n",
				 daemon_string(dt), name, addr );
		my_exit(1);
	}

	s.encode();
	if( !s.code(cmd) ) {
		fprintf( stderr, "Can't send command CONFIG_VAL (%d)\n", cmd );
		return NULL;
	}
	if( !s.code(param_name) ) {
		fprintf( stderr, "Can't send request (%s)\n", param_name );
		return NULL;
	}
	if( !s.end_of_message() ) {
		fprintf( stderr, "Can't send end of message\n" );
		return NULL;
	}

	s.decode();
	if( !s.code(val) ) {
		fprintf( stderr, "Can't receive reply from %s on %s %s\n",
				 daemon_string(dt), name, addr );
		return NULL;
	}
	if( !s.end_of_message() ) {
		fprintf( stderr, "Can't receive end of message\n" );
		return NULL;
	}
	return val;
}
<commit_msg>Added -set, -unset, -rset, and -runset options.  See condor_daemon_core.V6/README.config for more details.<commit_after>/***************************Copyright-DO-NOT-REMOVE-THIS-LINE**
 * CONDOR Copyright Notice
 *
 * See LICENSE.TXT for additional notices and disclaimers.
 *
 * Copyright (c)1990-1998 CONDOR Team, Computer Sciences Department,
 * University of Wisconsin-Madison, Madison, WI.  All Rights Reserved.
 * No use of the CONDOR Software Program Source Code is authorized
 * without the express consent of the CONDOR Team.  For more information
 * contact: CONDOR Team, Attention: Professor Miron Livny,
 * 7367 Computer Sciences, 1210 W. Dayton St., Madison, WI 53706-1685,
 * (608) 262-0856 or miron@cs.wisc.edu.
 *
 * U.S. Government Rights Restrictions: Use, duplication, or disclosure
 * by the U.S. Government is subject to restrictions as set forth in
 * subparagraph (c)(1)(ii) of The Rights in Technical Data and Computer
 * Software clause at DFARS 252.227-7013 or subparagraphs (c)(1) and
 * (2) of Commercial Computer Software-Restricted Rights at 48 CFR
 * 52.227-19, as applicable, CONDOR Team, Attention: Professor Miron
 * Livny, 7367 Computer Sciences, 1210 W. Dayton St., Madison,
 * WI 53706-1685, (608) 262-0856 or miron@cs.wisc.edu.
****************************Copyright-DO-NOT-REMOVE-THIS-LINE**/

/***********************************************************************
*
* Print declarations from the condor config files, condor_config and
* condor_config.local.  Declarations in files specified by
* LOCAL_CONFIG_FILE override those in the global config file, so this
* prints the same declarations found by condor programs using the
* config() routine.
*
* In addition, the configuration of remote machines can be queried.
* You can specify either a name or sinful string you want to connect
* to, what kind of daemon you want to query, and what pool to query to
* find the specified daemon.
*
***********************************************************************/

#include "condor_common.h"
#include "condor_config.h"
#include "condor_io.h"
#include "condor_uid.h"
#include "match_prefix.h"
#include "string_list.h"
#include "get_daemon_addr.h"
#include "daemon_types.h"

char	*MyName;
char	*mySubSystem = NULL;
StringList params;
daemonType dt = DT_MASTER;

// The pure-tools (PureCoverage, Purify, etc) spit out a bunch of
// stuff to stderr, which is where we normally put our error
// messages.  To enable config_val.test to produce easily readable
// output, even with pure-tools, we just write everything to stdout.
#if defined( PURE_DEBUG )
#	define stderr stdout
#endif

enum PrintType {CONDOR_OWNER, CONDOR_TILDE, CONDOR_NONE};
enum ModeType {CONDOR_QUERY, CONDOR_SET, CONDOR_UNSET,
			   CONDOR_RUNTIME_SET, CONDOR_RUNTIME_UNSET};

// On some systems, the output from config_val sometimes doesn't show
// up unless we explicitly flush before we exit.
void
my_exit( int status )
{
	fflush( stdout );
	fflush( stderr );
	exit( status );
}

void
usage()
{
	fprintf( stderr, "Usage: %s [options] variable [variable] ...\n", MyName );
	fprintf( stderr, "   or: %s [options] -set variable value ...\n",
			 MyName );
	fprintf( stderr, "   or: %s [options] -rset variable value ...\n",
			 MyName );
	fprintf( stderr, "   or: %s [options] -unset variable ...\n",
			 MyName );
	fprintf( stderr, "   or: %s [options] -runset variable ...\n",
			 MyName );
	fprintf( stderr, "   or: %s [options] -tilde\n", MyName );
	fprintf( stderr, "   or: %s [options] -owner\n", MyName );
	fprintf( stderr, "\n   Valid options are:\n" );
	fprintf( stderr, "   -name daemon_name\t(query the specified daemon for its configuration)\n" );
	fprintf( stderr, "   -pool hostname\t(use the given central manager to find daemons)\n" );
	fprintf( stderr, "   -address <ip:port>\t(connect to the given ip/port)\n" );
	fprintf( stderr, "   -host hostname\t(connect to the given hostname)\n" );
	fprintf( stderr, "   -set\t\t\t(set a persistent config file expression)\n" );
	fprintf( stderr, "   -rset\t\t(set a runtime config file expression\n" );

	fprintf( stderr, "   -unset\t\t(unset a persistent config file expression)\n" );
	fprintf( stderr, "   -runset\t\t(unset a runtime config file expression)\n" );

	fprintf( stderr, "   -master\t\t(query the master [default])\n" );
	fprintf( stderr, "   -schedd\t\t(query the schedd)\n" );
	fprintf( stderr, "   -startd\t\t(query the startd)\n" );
	fprintf( stderr, "   -collector\t\t(query the collector)\n" );
	fprintf( stderr, "   -negotiator\t\t(query the negotiator)\n" );
	fprintf( stderr, "   -tilde\t\t(return the path to the Condor home directory)\n" );
	fprintf( stderr, "   -owner\t\t(return the owner of the condor_config_val process)\n" );
	my_exit( 1 );
}

char* GetRemoteParam( char*, char*, char*, char* );
void  SetRemoteParam( char*, char*, char*, char*, char*, ModeType );

main( int argc, char* argv[] )
{
	char	*value, *tmp, *host = NULL;
	char	*addr = NULL, *name = NULL, *pool = NULL;
	int		i;

	PrintType pt = CONDOR_NONE;
	ModeType mt = CONDOR_QUERY;

	MyName = argv[0];

	for( i=1; i<argc; i++ ) {
		if( match_prefix( argv[i], "-host" ) ) {
			if( argv[i + 1] ) {
				host = strdup( argv[++i] );
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-name" ) ) {
			if( argv[i + 1] ) {
				i++;
				if( (tmp = get_daemon_name(argv[i])) ) {
					name = strdup( tmp );
				} else {
					fprintf( stderr, "%s: unknown host %s\n", MyName,
							 get_host_part(argv[i]) );
					my_exit( 1 );
				}
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-address" ) ) {
			if( argv[i + 1] ) {
				i++;
				if( is_valid_sinful(argv[i]) ) {
					addr = strdup( argv[i] );
				} else {
					fprintf( stderr, "%s: invalid address %s\n"
							 "Address must be of the form \"<111.222.333.444:555>\n"
							 "   where 111.222.333.444 is the ip address and"
							 "555 is the port\n  you wish to connect to (the"
							 "punctuation is important.\n", MyName, argv[i] );
					my_exit( 1 );
				}
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-pool" ) ) {
			if( argv[i + 1] ) {
				i++;
				if( (tmp = get_daemon_name(argv[i])) ) {
					pool = strdup( tmp );
				} else {
					fprintf( stderr, "%s: unknown host %s\n", MyName,
							 get_host_part(argv[i]) );
					my_exit( 1 );
				}
			} else {
				usage();
			}
		} else if( match_prefix( argv[i], "-owner" ) ) {
			pt = CONDOR_OWNER;
		} else if( match_prefix( argv[i], "-tilde" ) ) {
			pt = CONDOR_TILDE;
		} else if( match_prefix( argv[i], "-master" ) ) {
			dt = DT_MASTER;
		} else if( match_prefix( argv[i], "-schedd" ) ) {
			dt = DT_SCHEDD;
		} else if( match_prefix( argv[i], "-startd" ) ) {
			dt = DT_STARTD;
		} else if( match_prefix( argv[i], "-collector" ) ) {
			dt = DT_COLLECTOR;
		} else if( match_prefix( argv[i], "-negotiator" ) ) {
			dt = DT_NEGOTIATOR;
		} else if( match_prefix( argv[i], "-set" ) ) {
			mt = CONDOR_SET;
		} else if( match_prefix( argv[i], "-unset" ) ) {
			mt = CONDOR_UNSET;
		} else if( match_prefix( argv[i], "-rset" ) ) {
			mt = CONDOR_RUNTIME_SET;
		} else if( match_prefix( argv[i], "-runset" ) ) {
			mt = CONDOR_RUNTIME_UNSET;
		} else if( match_prefix( argv[i], "-" ) ) {
			usage();
		} else {
			params.append( strdup( argv[i] ) );
		}
	}

		// If we didn't get told what subsystem we should use, set it
		// to "TOOL".
	if( !mySubSystem ) {
		mySubSystem = strdup( "TOOL" );
	}

		// Want to do this before we try to find the address of a
		// remote daemon, since if there's no -pool option, we need to
		// param() for the COLLECTOR_HOST to contact.
	if( host ) {
		config_host( 0, host );
	} else {
		config( 0 );
	}

	if( name || pool || mt != CONDOR_QUERY ) {
		addr = get_daemon_addr( dt, name, pool );
		if( ! addr ) {
			fprintf( stderr, "Can't find address for %s %s\n",
					 daemon_string(dt), name );
			fprintf( stderr, "Perhaps you need to query another pool.\n" );
			my_exit( 1 );
		}
	}

	if( pt == CONDOR_TILDE ) {
		if( (tmp = get_tilde()) ) {
			printf( "%s\n", tmp );
			my_exit( 0 );
		} else {
			fprintf( stderr,
					 "Error: Specified -tilde but can't find %s\n",
					 "condor's home directory." );
			my_exit( 1 );
		}
	}

	if( pt == CONDOR_OWNER ) {
		printf( "%s\n", get_condor_username() );
		my_exit( 0 );
	}

	params.rewind();

	if( ! params.number() ) {
		usage();
	}

	while( (tmp = params.next()) ) {
		if( mt == CONDOR_SET || mt == CONDOR_RUNTIME_SET ) {
			value = params.next();
			SetRemoteParam( name, addr, pool, tmp, value, mt );
		} else if( mt == CONDOR_UNSET || mt == CONDOR_RUNTIME_UNSET ) {
			SetRemoteParam( name, addr, pool, tmp, "", mt );
		} else {
			if( name || pool || addr ) {
				value = GetRemoteParam( name, addr, pool, tmp );
			} else {
				value = param( tmp );
			}
			if( value == NULL ) {
				fprintf(stderr, "Not defined: %s\n", tmp);
				my_exit( 1 );
			} else {
				printf("%s\n", value);
				free( value );
			}
		}
	}
	my_exit( 0 );
}

char*
GetRemoteParam( char* name, char* addr, char* pool, char* param_name )
{
	ReliSock s;
	s.timeout( 30 );
	char	*val = NULL;
	int 	cmd = CONFIG_VAL;

	if( !name ) {
		name = "";
	}

	if( ! s.connect( addr, 0 ) ) {
		fprintf( stderr, "Can't connect to %s on %s %s\n",
				 daemon_string(dt), name, addr );
		my_exit(1);
	}

	s.encode();
	if( !s.code(cmd) ) {
		fprintf( stderr, "Can't send command CONFIG_VAL (%d)\n", cmd );
		return NULL;
	}
	if( !s.code(param_name) ) {
		fprintf( stderr, "Can't send request (%s)\n", param_name );
		return NULL;
	}
	if( !s.end_of_message() ) {
		fprintf( stderr, "Can't send end of message\n" );
		return NULL;
	}

	s.decode();
	if( !s.code(val) ) {
		fprintf( stderr, "Can't receive reply from %s on %s %s\n",
				 daemon_string(dt), name, addr );
		return NULL;
	}
	if( !s.end_of_message() ) {
		fprintf( stderr, "Can't receive end of message\n" );
		return NULL;
	}
	return val;
}

void
SetRemoteParam( char* name, char* addr, char* pool, char* param_name,
				char* param_value, ModeType mt )
{
	int cmd, rval;
	ReliSock s;

	s.timeout( 30 );

	switch (mt) {
	case CONDOR_SET:
	case CONDOR_UNSET:
		cmd = DC_CONFIG_PERSIST;
		break;
	case CONDOR_RUNTIME_SET:
	case CONDOR_RUNTIME_UNSET:
		cmd = DC_CONFIG_RUNTIME;
		break;
	default:
		fprintf( stderr, "Unknown command type %d\n", (int)mt );
		my_exit( 1 );
	}

	if( !name ) {
		name = "";
	}

	if( ! s.connect( addr, 0 ) ) {
		fprintf( stderr, "Can't connect to %s on %s %s\n",
				 daemon_string(dt), name, addr );
		my_exit(1);
	}

	s.encode();
	if( !s.code(cmd) ) {
		fprintf( stderr, "Can't send DC_CONFIG command (%d)\n", cmd );
		my_exit(1);
	}
	if( !s.code(param_name) ) {
		fprintf( stderr, "Can't send param name (%s)\n", param_name );
		my_exit(1);
	}
	char *buf = NULL;
	if (param_value && param_value[0]) {
		buf = (char *)malloc(strlen(param_name)+strlen(param_value)+5);
		sprintf(buf, "%s : %s\n", param_name, param_value);
		if( !s.code(buf) ) {
			fprintf( stderr, "Can't send config setting (%s)\n", buf );
			free(buf);
			my_exit(1);
		}
	} else {
		if( !s.put("") ) {
			fprintf( stderr, "Can't send config setting\n" );
			my_exit(1);
		}
	}
	if( !s.end_of_message() ) {
		fprintf( stderr, "Can't send end of message\n" );
		if (buf) free(buf);
		my_exit(1);
	}

	s.decode();
	if( !s.code(rval) ) {
		fprintf( stderr, "Can't receive reply from %s on %s %s\n",
				 daemon_string(dt), name, addr );
		if (buf) free(buf);
		my_exit(1);
	}
	if( !s.end_of_message() ) {
		fprintf( stderr, "Can't receive end of message\n" );
		if (buf) free(buf);
		my_exit(1);
	}
	if (buf) buf[strlen(buf)-1] = '\0';	// remove newline
	if (rval < 0) {
		if (buf) {
			fprintf( stderr, "Attempt to set configuration \"%s\" on %s %s "
					 "%s failed.\n",
					 buf, daemon_string(dt), name, addr );
			free(buf);
		} else {
			fprintf( stderr, "Attempt to unset configuration \"%s\" on %s %s "
					 "%s failed.\n",
					 param_name, daemon_string(dt), name, addr );
		}
		my_exit(1);
	}
	if (buf) {
		fprintf( stderr, "Successfully set configuration \"%s\" on %s %s "
				 "%s.\n",
				 buf, daemon_string(dt), name, addr );
		free(buf);
	} else {
		fprintf( stderr, "Successfully unset configuration \"%s\" on %s %s "
				 "%s.\n",
				 param_name, daemon_string(dt), name, addr );
	}
}
<|endoftext|>


<commit_before>#ifdef _WIN32

#include "win_fsnotifier.h"

using namespace std;

//
// WatchPoint
//

WatchPoint::WatchPoint(Server* server, size_t bufferSize, const u16string& path)
    : path(path)
    , status(NOT_LISTENING) {
    wstring pathW(path.begin(), path.end());
    HANDLE directoryHandle = CreateFileW(
        pathW.c_str(),          // pointer to the file name
        FILE_LIST_DIRECTORY,    // access (read/write) mode
        CREATE_SHARE,           // share mode
        NULL,                   // security descriptor
        OPEN_EXISTING,          // how to create
        CREATE_FLAGS,           // file attributes
        NULL                    // file with attributes to copy
    );
    if (directoryHandle == INVALID_HANDLE_VALUE) {
        throw FileWatcherException("Couldn't add watch", path, GetLastError());
    }
    this->directoryHandle = directoryHandle;

    this->server = server;
    this->buffer.reserve(bufferSize);
    ZeroMemory(&this->overlapped, sizeof(OVERLAPPED));
    this->overlapped.hEvent = this;
    switch (listen()) {
        case SUCCESS:
            break;
        case DELETED:
            throw FileWatcherException("Couldn't start watching because path is not a directory", path);
    }
}

bool WatchPoint::cancel() {
    if (status == LISTENING) {
        logToJava(FINE, "Cancelling %s", utf16ToUtf8String(path).c_str());
        status = CANCELLED;
        bool cancelled = (bool) CancelIoEx(directoryHandle, &overlapped);
        if (!cancelled) {
            DWORD cancelError = GetLastError();
            close();
            if (cancelError == ERROR_NOT_FOUND) {
                // Do nothing, looks like this is a typical scenario
                logToJava(FINE, "Watch point already finished %s", utf16ToUtf8String(path).c_str());
            } else {
                throw FileWatcherException("Couldn't cancel watch point", path, cancelError);
            }
        }
        return cancelled;
    }
    return false;
}

WatchPoint::~WatchPoint() {
    try {
        if (cancel()) {
            SleepEx(0, true);
        }
        close();
    } catch (const exception& ex) {
        logToJava(WARNING, "Couldn't cancel watch point %s: %s", utf16ToUtf8String(path).c_str(), ex.what());
    }
}

static void CALLBACK handleEventCallback(DWORD errorCode, DWORD bytesTransferred, LPOVERLAPPED overlapped) {
    WatchPoint* watchPoint = (WatchPoint*) overlapped->hEvent;
    watchPoint->handleEventsInBuffer(errorCode, bytesTransferred);
}

bool WatchPoint::isValidDirectory() {
    wstring pathW(path.begin(), path.end());
    DWORD attrib = GetFileAttributesW(pathW.c_str());

    return (attrib != INVALID_FILE_ATTRIBUTES)
        && ((attrib & FILE_ATTRIBUTE_DIRECTORY) != 0);
}

ListenResult WatchPoint::listen() {
    BOOL success = ReadDirectoryChangesW(
        directoryHandle,              // handle to directory
        &buffer[0],                   // read results buffer
        (DWORD) buffer.capacity(),    // length of buffer
        TRUE,                         // include children
        EVENT_MASK,                   // filter conditions
        NULL,                         // bytes returned
        &overlapped,                  // overlapped buffer
        &handleEventCallback          // completion routine
    );
    if (success) {
        status = LISTENING;
        return SUCCESS;
    } else {
        DWORD listenError = GetLastError();
        close();
        if (listenError == ERROR_ACCESS_DENIED && !isValidDirectory()) {
            return DELETED;
        } else {
            throw FileWatcherException("Couldn't start watching", path, listenError);
        }
    }
}

void WatchPoint::close() {
    if (status != FINISHED) {
        BOOL ret = CloseHandle(directoryHandle);
        if (!ret) {
            logToJava(SEVERE, "Couldn't close handle %p for '%ls': %d", directoryHandle, utf16ToUtf8String(path).c_str(), GetLastError());
        }
        status = FINISHED;
    }
}

void WatchPoint::handleEventsInBuffer(DWORD errorCode, DWORD bytesTransferred) {
    if (errorCode == ERROR_OPERATION_ABORTED) {
        logToJava(FINE, "Finished watching '%s', status = %d", utf16ToUtf8String(path).c_str(), status);
        close();
        return;
    }

    if (status != LISTENING) {
        logToJava(FINE, "Ignoring incoming events for %s as watch-point is not listening (%d bytes, errorCode = %d, status = %d)",
            utf16ToUtf8String(path).c_str(), bytesTransferred, errorCode, status);
        return;
    }
    status = NOT_LISTENING;
    server->handleEvents(this, errorCode, buffer, bytesTransferred);
}

void Server::handleEvents(WatchPoint* watchPoint, DWORD errorCode, const vector<BYTE>& buffer, DWORD bytesTransferred) {
    unique_lock<mutex> lock(mutationMutex);
    JNIEnv* env = getThreadEnv();
    const u16string& path = watchPoint->path;

    try {
        if (errorCode != ERROR_SUCCESS) {
            if (errorCode == ERROR_ACCESS_DENIED && !watchPoint->isValidDirectory()) {
                reportChange(env, REMOVED, path);
                watchPoint->close();
                return;
            } else {
                throw FileWatcherException("Error received when handling events", path, errorCode);
            }
        }

        if (terminated) {
            logToJava(FINE, "Ignoring incoming events for %s because server is terminating (%d bytes, status = %d)",
                utf16ToUtf8String(path).c_str(), bytesTransferred, watchPoint->status);
            return;
        }

        if (bytesTransferred == 0) {
            // This is what the documentation has to say about a zero-length dataset:
            //
            //     If the number of bytes transferred is zero, the buffer was either too large
            //     for the system to allocate or too small to provide detailed information on
            //     all the changes that occurred in the directory or subtree. In this case,
            //     you should compute the changes by enumerating the directory or subtree.
            //
            // (See https://docs.microsoft.com/en-us/windows/win32/api/winbase/nf-winbase-readdirectorychangesw)
            //
            // We'll handle this as a simple overflow and report it as such.
            logToJava(INFO, "Detected overflow for %s", utf16ToUtf8String(path).c_str());
            reportChange(env, OVERFLOWED, path);
        } else {
            int index = 0;
            for (;;) {
                FILE_NOTIFY_INFORMATION* current = (FILE_NOTIFY_INFORMATION*) &buffer[index];
                handleEvent(env, path, current);
                if (current->NextEntryOffset == 0) {
                    break;
                }
                index += current->NextEntryOffset;
            }
        }

        switch (watchPoint->listen()) {
            case SUCCESS:
                break;
            case DELETED:
                logToJava(FINE, "Watched directory removed for %s", utf16ToUtf8String(path).c_str());
                reportChange(env, REMOVED, path);
                break;
        }
    } catch (const exception& ex) {
        reportError(env, ex);
    }
}

bool isAbsoluteLocalPath(const u16string& path) {
    if (path.length() < 3) {
        return false;
    }
    return ((u'a' <= path[0] && path[0] <= u'z') || (u'A' <= path[0] && path[0] <= u'Z'))
        && path[1] == u':'
        && path[2] == u'\\';
}

bool isAbsoluteUncPath(const u16string& path) {
    if (path.length() < 3) {
        return false;
    }
    return path[0] == u'\\' && path[1] == u'\\';
}

bool isLongPath(const u16string& path) {
    return path.length() >= 4 && path.substr(0, 4) == u"\\\\?\\";
}

bool isUncLongPath(const u16string& path) {
    return path.length() >= 8 && path.substr(0, 8) == u"\\\\?\\UNC\\";
}

// TODO How can this be done nicer, wihtout both unnecessary copy and in-place mutation?
void convertToLongPathIfNeeded(u16string& path) {
    // Technically, this should be MAX_PATH (i.e. 260), except some Win32 API related
    // to working with directory paths are actually limited to 240. It is just
    // safer/simpler to cover both cases in one code path.
    if (path.length() <= 240) {
        return;
    }

    // It is already a long path, nothing to do here
    if (isLongPath(path)) {
        return;
    }

    if (isAbsoluteLocalPath(path)) {
        // Format: C:\... -> \\?\C:\...
        path.insert(0, u"\\\\?\\");
    } else if (isAbsoluteUncPath(path)) {
        // In this case, we need to skip the first 2 characters:
        // Format: \\server\share\... -> \\?\UNC\server\share\...
        path.erase(0, 2);
        path.insert(0, u"\\\\?\\UNC\\");
    } else {
        // It is some sort of unknown format, don't mess with it
    }
}

void Server::handleEvent(JNIEnv* env, const u16string& path, FILE_NOTIFY_INFORMATION* info) {
    wstring changedPathW = wstring(info->FileName, 0, info->FileNameLength / sizeof(wchar_t));
    u16string changedPath(changedPathW.begin(), changedPathW.end());
    if (!changedPath.empty()) {
        changedPath.insert(0, 1, u'\\');
    }
    changedPath.insert(0, path);
    // TODO Remove long prefix for path once?
    if (isLongPath(changedPath)) {
        if (isUncLongPath(changedPath)) {
            changedPath.erase(0, 8).insert(0, u"\\\\");
        } else {
            changedPath.erase(0, 4);
        }
    }

    logToJava(FINE, "Change detected: 0x%x '%s'", info->Action, utf16ToUtf8String(changedPath).c_str());

    FileWatchEventType type;
    if (info->Action == FILE_ACTION_ADDED || info->Action == FILE_ACTION_RENAMED_NEW_NAME) {
        type = CREATED;
    } else if (info->Action == FILE_ACTION_REMOVED || info->Action == FILE_ACTION_RENAMED_OLD_NAME) {
        type = REMOVED;
    } else if (info->Action == FILE_ACTION_MODIFIED) {
        type = MODIFIED;
    } else {
        logToJava(WARNING, "Unknown event 0x%x for %s", info->Action, utf16ToUtf8String(changedPath).c_str());
        type = UNKNOWN;
    }

    reportChange(env, type, changedPath);
}

//
// Server
//

Server::Server(JNIEnv* env, size_t bufferSize, jobject watcherCallback)
    : AbstractServer(env, watcherCallback)
    , bufferSize(bufferSize) {
}

void Server::initializeRunLoop() {
    // TODO For some reason GetCurrentThread() returns a thread that doesn't accept APCs
    threadHandle = OpenThread(
        THREAD_ALL_ACCESS,      // dwDesiredAccess
        false,                  // bInheritHandle
        GetCurrentThreadId()    // dwThreadId
    );
    if (threadHandle == NULL) {
        throw FileWatcherException("Couldn't open current thread", GetLastError());
    }
}

void Server::terminateRunLoop() {
    executeOnRunLoop([this]() {
        terminated = true;
        return true;
    });
}

void Server::runLoop() {
    while (!terminated) {
        SleepEx(INFINITE, true);
    }

    // We have received termination, cancel all watchers
    unique_lock<mutex> lock(mutationMutex);
    logToJava(FINE, "Finished with run loop, now cancelling remaining watch points", NULL);
    int pendingWatchPoints = 0;
    for (auto& it : watchPoints) {
        auto& watchPoint = it.second;
        switch (watchPoint.status) {
            case LISTENING:
                try {
                    if (watchPoint.cancel()) {
                        pendingWatchPoints++;
                    }
                } catch (const exception& ex) {
                    logToJava(SEVERE, "%s", ex.what());
                }
                break;
            case CANCELLED:
                pendingWatchPoints++;
                break;
            default:
                break;
        }
    }

    // If there are any pending watchers, wait for them to finish
    if (pendingWatchPoints > 0) {
        logToJava(FINE, "Waiting for %d pending watch points to finish", pendingWatchPoints);
        SleepEx(0, true);
    }

    // Warn about  any unfinished watchpoints
    for (auto& it : watchPoints) {
        auto& watchPoint = it.second;
        switch (watchPoint.status) {
            case NOT_LISTENING:
            case FINISHED:
                break;
            default:
                logToJava(WARNING, "Watch point %s did not finish before termination timeout (status = %d)",
                    utf16ToUtf8String(watchPoint.path).c_str(), watchPoint.status);
                break;
        }
    }

    CloseHandle(threadHandle);
}

struct Command {
    Server* server;
    function<bool()> function;
    condition_variable executed;
    bool result;
    exception_ptr failure;
};

static void CALLBACK executeOnRunLoopCallback(_In_ ULONG_PTR info) {
    Command* command = (Command*) info;
    try {
        command->result = command->function();
    } catch (const exception&) {
        command->failure = current_exception();
    }
    unique_lock<mutex> lock(command->server->executionMutex);
    command->executed.notify_all();
}

bool Server::executeOnRunLoop(function<bool()> function) {
    Command command;
    command.function = function;
    command.server = this;
    unique_lock<mutex> lock(executionMutex);
    DWORD ret = QueueUserAPC(executeOnRunLoopCallback, threadHandle, (ULONG_PTR) &command);
    if (ret == 0) {
        throw FileWatcherException("Received error while queuing APC", GetLastError());
    }
    auto status = command.executed.wait_for(lock, THREAD_TIMEOUT);
    if (status == cv_status::timeout) {
        throw FileWatcherException("Execution timed out");
    } else if (command.failure) {
        rethrow_exception(command.failure);
    } else {
        return command.result;
    }
}

void Server::registerPaths(const vector<u16string>& paths) {
    executeOnRunLoop([this, paths]() {
        AbstractServer::registerPaths(paths);
        return true;
    });
}

bool Server::unregisterPaths(const vector<u16string>& paths) {
    return executeOnRunLoop([this, paths]() {
        return AbstractServer::unregisterPaths(paths);
    });
}

void Server::registerPath(const u16string& path) {
    u16string longPath = path;
    convertToLongPathIfNeeded(longPath);
    auto it = watchPoints.find(longPath);
    if (it != watchPoints.end()) {
        if (it->second.status != FINISHED) {
            throw FileWatcherException("Already watching path", path);
        }
        watchPoints.erase(it);
    }
    watchPoints.emplace(piecewise_construct,
        forward_as_tuple(longPath),
        forward_as_tuple(this, bufferSize, longPath));
}

bool Server::unregisterPath(const u16string& path) {
    u16string longPath = path;
    convertToLongPathIfNeeded(longPath);
    if (watchPoints.erase(longPath) == 0) {
        logToJava(INFO, "Path is not watched: %s", utf16ToUtf8String(path).c_str());
        return false;
    }
    return true;
}

//
// JNI calls
//

JNIEXPORT jobject JNICALL
Java_net_rubygrapefruit_platform_internal_jni_WindowsFileEventFunctions_startWatcher0(JNIEnv* env, jclass target, jint bufferSize, jobject javaCallback) {
    return wrapServer(env, new Server(env, bufferSize, javaCallback));
}

#endif
<commit_msg>Polish cancel state change<commit_after>#ifdef _WIN32

#include "win_fsnotifier.h"

using namespace std;

//
// WatchPoint
//

WatchPoint::WatchPoint(Server* server, size_t bufferSize, const u16string& path)
    : path(path)
    , status(NOT_LISTENING) {
    wstring pathW(path.begin(), path.end());
    HANDLE directoryHandle = CreateFileW(
        pathW.c_str(),          // pointer to the file name
        FILE_LIST_DIRECTORY,    // access (read/write) mode
        CREATE_SHARE,           // share mode
        NULL,                   // security descriptor
        OPEN_EXISTING,          // how to create
        CREATE_FLAGS,           // file attributes
        NULL                    // file with attributes to copy
    );
    if (directoryHandle == INVALID_HANDLE_VALUE) {
        throw FileWatcherException("Couldn't add watch", path, GetLastError());
    }
    this->directoryHandle = directoryHandle;

    this->server = server;
    this->buffer.reserve(bufferSize);
    ZeroMemory(&this->overlapped, sizeof(OVERLAPPED));
    this->overlapped.hEvent = this;
    switch (listen()) {
        case SUCCESS:
            break;
        case DELETED:
            throw FileWatcherException("Couldn't start watching because path is not a directory", path);
    }
}

bool WatchPoint::cancel() {
    if (status == LISTENING) {
        logToJava(FINE, "Cancelling %s", utf16ToUtf8String(path).c_str());
        bool cancelled = (bool) CancelIoEx(directoryHandle, &overlapped);
        if (cancelled) {
            status = CANCELLED;
        } else {
            DWORD cancelError = GetLastError();
            close();
            if (cancelError == ERROR_NOT_FOUND) {
                // Do nothing, looks like this is a typical scenario
                logToJava(FINE, "Watch point already finished %s", utf16ToUtf8String(path).c_str());
            } else {
                throw FileWatcherException("Couldn't cancel watch point", path, cancelError);
            }
        }
        return cancelled;
    }
    return false;
}

WatchPoint::~WatchPoint() {
    try {
        if (cancel()) {
            SleepEx(0, true);
        }
        close();
    } catch (const exception& ex) {
        logToJava(WARNING, "Couldn't cancel watch point %s: %s", utf16ToUtf8String(path).c_str(), ex.what());
    }
}

static void CALLBACK handleEventCallback(DWORD errorCode, DWORD bytesTransferred, LPOVERLAPPED overlapped) {
    WatchPoint* watchPoint = (WatchPoint*) overlapped->hEvent;
    watchPoint->handleEventsInBuffer(errorCode, bytesTransferred);
}

bool WatchPoint::isValidDirectory() {
    wstring pathW(path.begin(), path.end());
    DWORD attrib = GetFileAttributesW(pathW.c_str());

    return (attrib != INVALID_FILE_ATTRIBUTES)
        && ((attrib & FILE_ATTRIBUTE_DIRECTORY) != 0);
}

ListenResult WatchPoint::listen() {
    BOOL success = ReadDirectoryChangesW(
        directoryHandle,              // handle to directory
        &buffer[0],                   // read results buffer
        (DWORD) buffer.capacity(),    // length of buffer
        TRUE,                         // include children
        EVENT_MASK,                   // filter conditions
        NULL,                         // bytes returned
        &overlapped,                  // overlapped buffer
        &handleEventCallback          // completion routine
    );
    if (success) {
        status = LISTENING;
        return SUCCESS;
    } else {
        DWORD listenError = GetLastError();
        close();
        if (listenError == ERROR_ACCESS_DENIED && !isValidDirectory()) {
            return DELETED;
        } else {
            throw FileWatcherException("Couldn't start watching", path, listenError);
        }
    }
}

void WatchPoint::close() {
    if (status != FINISHED) {
        BOOL ret = CloseHandle(directoryHandle);
        if (!ret) {
            logToJava(SEVERE, "Couldn't close handle %p for '%ls': %d", directoryHandle, utf16ToUtf8String(path).c_str(), GetLastError());
        }
        status = FINISHED;
    }
}

void WatchPoint::handleEventsInBuffer(DWORD errorCode, DWORD bytesTransferred) {
    if (errorCode == ERROR_OPERATION_ABORTED) {
        logToJava(FINE, "Finished watching '%s', status = %d", utf16ToUtf8String(path).c_str(), status);
        close();
        return;
    }

    if (status != LISTENING) {
        logToJava(FINE, "Ignoring incoming events for %s as watch-point is not listening (%d bytes, errorCode = %d, status = %d)",
            utf16ToUtf8String(path).c_str(), bytesTransferred, errorCode, status);
        return;
    }
    status = NOT_LISTENING;
    server->handleEvents(this, errorCode, buffer, bytesTransferred);
}

void Server::handleEvents(WatchPoint* watchPoint, DWORD errorCode, const vector<BYTE>& buffer, DWORD bytesTransferred) {
    unique_lock<mutex> lock(mutationMutex);
    JNIEnv* env = getThreadEnv();
    const u16string& path = watchPoint->path;

    try {
        if (errorCode != ERROR_SUCCESS) {
            if (errorCode == ERROR_ACCESS_DENIED && !watchPoint->isValidDirectory()) {
                reportChange(env, REMOVED, path);
                watchPoint->close();
                return;
            } else {
                throw FileWatcherException("Error received when handling events", path, errorCode);
            }
        }

        if (terminated) {
            logToJava(FINE, "Ignoring incoming events for %s because server is terminating (%d bytes, status = %d)",
                utf16ToUtf8String(path).c_str(), bytesTransferred, watchPoint->status);
            return;
        }

        if (bytesTransferred == 0) {
            // This is what the documentation has to say about a zero-length dataset:
            //
            //     If the number of bytes transferred is zero, the buffer was either too large
            //     for the system to allocate or too small to provide detailed information on
            //     all the changes that occurred in the directory or subtree. In this case,
            //     you should compute the changes by enumerating the directory or subtree.
            //
            // (See https://docs.microsoft.com/en-us/windows/win32/api/winbase/nf-winbase-readdirectorychangesw)
            //
            // We'll handle this as a simple overflow and report it as such.
            logToJava(INFO, "Detected overflow for %s", utf16ToUtf8String(path).c_str());
            reportChange(env, OVERFLOWED, path);
        } else {
            int index = 0;
            for (;;) {
                FILE_NOTIFY_INFORMATION* current = (FILE_NOTIFY_INFORMATION*) &buffer[index];
                handleEvent(env, path, current);
                if (current->NextEntryOffset == 0) {
                    break;
                }
                index += current->NextEntryOffset;
            }
        }

        switch (watchPoint->listen()) {
            case SUCCESS:
                break;
            case DELETED:
                logToJava(FINE, "Watched directory removed for %s", utf16ToUtf8String(path).c_str());
                reportChange(env, REMOVED, path);
                break;
        }
    } catch (const exception& ex) {
        reportError(env, ex);
    }
}

bool isAbsoluteLocalPath(const u16string& path) {
    if (path.length() < 3) {
        return false;
    }
    return ((u'a' <= path[0] && path[0] <= u'z') || (u'A' <= path[0] && path[0] <= u'Z'))
        && path[1] == u':'
        && path[2] == u'\\';
}

bool isAbsoluteUncPath(const u16string& path) {
    if (path.length() < 3) {
        return false;
    }
    return path[0] == u'\\' && path[1] == u'\\';
}

bool isLongPath(const u16string& path) {
    return path.length() >= 4 && path.substr(0, 4) == u"\\\\?\\";
}

bool isUncLongPath(const u16string& path) {
    return path.length() >= 8 && path.substr(0, 8) == u"\\\\?\\UNC\\";
}

// TODO How can this be done nicer, wihtout both unnecessary copy and in-place mutation?
void convertToLongPathIfNeeded(u16string& path) {
    // Technically, this should be MAX_PATH (i.e. 260), except some Win32 API related
    // to working with directory paths are actually limited to 240. It is just
    // safer/simpler to cover both cases in one code path.
    if (path.length() <= 240) {
        return;
    }

    // It is already a long path, nothing to do here
    if (isLongPath(path)) {
        return;
    }

    if (isAbsoluteLocalPath(path)) {
        // Format: C:\... -> \\?\C:\...
        path.insert(0, u"\\\\?\\");
    } else if (isAbsoluteUncPath(path)) {
        // In this case, we need to skip the first 2 characters:
        // Format: \\server\share\... -> \\?\UNC\server\share\...
        path.erase(0, 2);
        path.insert(0, u"\\\\?\\UNC\\");
    } else {
        // It is some sort of unknown format, don't mess with it
    }
}

void Server::handleEvent(JNIEnv* env, const u16string& path, FILE_NOTIFY_INFORMATION* info) {
    wstring changedPathW = wstring(info->FileName, 0, info->FileNameLength / sizeof(wchar_t));
    u16string changedPath(changedPathW.begin(), changedPathW.end());
    if (!changedPath.empty()) {
        changedPath.insert(0, 1, u'\\');
    }
    changedPath.insert(0, path);
    // TODO Remove long prefix for path once?
    if (isLongPath(changedPath)) {
        if (isUncLongPath(changedPath)) {
            changedPath.erase(0, 8).insert(0, u"\\\\");
        } else {
            changedPath.erase(0, 4);
        }
    }

    logToJava(FINE, "Change detected: 0x%x '%s'", info->Action, utf16ToUtf8String(changedPath).c_str());

    FileWatchEventType type;
    if (info->Action == FILE_ACTION_ADDED || info->Action == FILE_ACTION_RENAMED_NEW_NAME) {
        type = CREATED;
    } else if (info->Action == FILE_ACTION_REMOVED || info->Action == FILE_ACTION_RENAMED_OLD_NAME) {
        type = REMOVED;
    } else if (info->Action == FILE_ACTION_MODIFIED) {
        type = MODIFIED;
    } else {
        logToJava(WARNING, "Unknown event 0x%x for %s", info->Action, utf16ToUtf8String(changedPath).c_str());
        type = UNKNOWN;
    }

    reportChange(env, type, changedPath);
}

//
// Server
//

Server::Server(JNIEnv* env, size_t bufferSize, jobject watcherCallback)
    : AbstractServer(env, watcherCallback)
    , bufferSize(bufferSize) {
}

void Server::initializeRunLoop() {
    // TODO For some reason GetCurrentThread() returns a thread that doesn't accept APCs
    threadHandle = OpenThread(
        THREAD_ALL_ACCESS,      // dwDesiredAccess
        false,                  // bInheritHandle
        GetCurrentThreadId()    // dwThreadId
    );
    if (threadHandle == NULL) {
        throw FileWatcherException("Couldn't open current thread", GetLastError());
    }
}

void Server::terminateRunLoop() {
    executeOnRunLoop([this]() {
        terminated = true;
        return true;
    });
}

void Server::runLoop() {
    while (!terminated) {
        SleepEx(INFINITE, true);
    }

    // We have received termination, cancel all watchers
    unique_lock<mutex> lock(mutationMutex);
    logToJava(FINE, "Finished with run loop, now cancelling remaining watch points", NULL);
    int pendingWatchPoints = 0;
    for (auto& it : watchPoints) {
        auto& watchPoint = it.second;
        switch (watchPoint.status) {
            case LISTENING:
                try {
                    if (watchPoint.cancel()) {
                        pendingWatchPoints++;
                    }
                } catch (const exception& ex) {
                    logToJava(SEVERE, "%s", ex.what());
                }
                break;
            case CANCELLED:
                pendingWatchPoints++;
                break;
            default:
                break;
        }
    }

    // If there are any pending watchers, wait for them to finish
    if (pendingWatchPoints > 0) {
        logToJava(FINE, "Waiting for %d pending watch points to finish", pendingWatchPoints);
        SleepEx(0, true);
    }

    // Warn about  any unfinished watchpoints
    for (auto& it : watchPoints) {
        auto& watchPoint = it.second;
        switch (watchPoint.status) {
            case NOT_LISTENING:
            case FINISHED:
                break;
            default:
                logToJava(WARNING, "Watch point %s did not finish before termination timeout (status = %d)",
                    utf16ToUtf8String(watchPoint.path).c_str(), watchPoint.status);
                break;
        }
    }

    CloseHandle(threadHandle);
}

struct Command {
    Server* server;
    function<bool()> function;
    condition_variable executed;
    bool result;
    exception_ptr failure;
};

static void CALLBACK executeOnRunLoopCallback(_In_ ULONG_PTR info) {
    Command* command = (Command*) info;
    try {
        command->result = command->function();
    } catch (const exception&) {
        command->failure = current_exception();
    }
    unique_lock<mutex> lock(command->server->executionMutex);
    command->executed.notify_all();
}

bool Server::executeOnRunLoop(function<bool()> function) {
    Command command;
    command.function = function;
    command.server = this;
    unique_lock<mutex> lock(executionMutex);
    DWORD ret = QueueUserAPC(executeOnRunLoopCallback, threadHandle, (ULONG_PTR) &command);
    if (ret == 0) {
        throw FileWatcherException("Received error while queuing APC", GetLastError());
    }
    auto status = command.executed.wait_for(lock, THREAD_TIMEOUT);
    if (status == cv_status::timeout) {
        throw FileWatcherException("Execution timed out");
    } else if (command.failure) {
        rethrow_exception(command.failure);
    } else {
        return command.result;
    }
}

void Server::registerPaths(const vector<u16string>& paths) {
    executeOnRunLoop([this, paths]() {
        AbstractServer::registerPaths(paths);
        return true;
    });
}

bool Server::unregisterPaths(const vector<u16string>& paths) {
    return executeOnRunLoop([this, paths]() {
        return AbstractServer::unregisterPaths(paths);
    });
}

void Server::registerPath(const u16string& path) {
    u16string longPath = path;
    convertToLongPathIfNeeded(longPath);
    auto it = watchPoints.find(longPath);
    if (it != watchPoints.end()) {
        if (it->second.status != FINISHED) {
            throw FileWatcherException("Already watching path", path);
        }
        watchPoints.erase(it);
    }
    watchPoints.emplace(piecewise_construct,
        forward_as_tuple(longPath),
        forward_as_tuple(this, bufferSize, longPath));
}

bool Server::unregisterPath(const u16string& path) {
    u16string longPath = path;
    convertToLongPathIfNeeded(longPath);
    if (watchPoints.erase(longPath) == 0) {
        logToJava(INFO, "Path is not watched: %s", utf16ToUtf8String(path).c_str());
        return false;
    }
    return true;
}

//
// JNI calls
//

JNIEXPORT jobject JNICALL
Java_net_rubygrapefruit_platform_internal_jni_WindowsFileEventFunctions_startWatcher0(JNIEnv* env, jclass target, jint bufferSize, jobject javaCallback) {
    return wrapServer(env, new Server(env, bufferSize, javaCallback));
}

#endif
<|endoftext|>