<commit_before>/// @file
/// @author uentity
/// @date 20.11.2017
/// @brief Symbolic link implementation
/// @copyright
/// This Source Code Form is subject to the terms of the Mozilla Public License,
/// v. 2.0. If a copy of the MPL was not distributed with this file,
/// You can obtain one at https://mozilla.org/MPL/2.0/

#include <bs/log.h>
#include <bs/tree/tree.h>
#include <bs/tree/errors.h>
#include <bs/kernel/config.h>
#include <bs/kernel/tools.h>

#include <bs/serialize/tree.h>
#include <bs/serialize/cafbind.h>

#include "link_actor.h"

OMIT_OBJ_SERIALIZATION

NAMESPACE_BEGIN(blue_sky::tree)
///////////////////////////////////////////////////////////////////////////////
//  actor + impl
//

// actor
struct sym_link_actor : public link_actor {
	using super = link_actor;

	using super::super;

	auto pointee() const -> result_or_err<sp_link> {
		return static_cast<sym_link_impl&>(impl).pointee();
	}

	//// make data request via source link
	//auto data_ex(obj_processor_f cb, ReqOpts opts) -> void override {
	//	pointee().and_then([&](const sp_link& src_link) {
	//		if(enumval(opts & ReqOpts::DirectInvoke))
	//			cb(src_link->data_ex());
	//		else
	//			caf::anon_send(src_link->actor(), a_lnk_data(), std::move(cb), opts);
	//	});
	//}

	//auto data_node_ex(node_processor_f cb, ReqOpts opts) -> void override {
	//	pointee().and_then([&](const sp_link& src_link) {
	//		if(enumval(opts & ReqOpts::DirectInvoke))
	//			cb(src_link->data_node_ex());
	//		else
	//			caf::anon_send(src_link->actor(), a_lnk_dnode(), std::move(cb), opts);
	//	});
	//}

	template<typename R, typename... Args>
	auto delegate_source(Args&&... args)
	-> std::enable_if_t<tl::detail::is_expected<R>::value, caf::response_promise> {
		auto res = make_response_promise<R>();
		if(auto p = pointee())
			res.delegate(link::actor(*p.value()), std::forward<Args>(args)...);
		else
			res.deliver(R{ tl::unexpect, p.error() });
		return res;
	}

	template<typename R, typename... Args>
	auto delegate_source(R errval, Args&&... args)
	-> std::enable_if_t<!tl::detail::is_expected<R>::value, caf::response_promise> {
		auto res = make_response_promise<R>();
		if(auto p = pointee())
			res.delegate(link::actor(*p.value()), std::forward<Args>(args)...);
		else
			res.deliver(std::move(errval));
		return res;
	}

	// delegate name, OID, etc requests to source link
	auto make_behavior() -> caf::behavior override {
		return caf::message_handler({

			[this](a_lnk_oid) -> caf::result<std::string> {
				return delegate_source<std::string>(type_descriptor::nil().name, a_lnk_oid());
			},

			[this](a_lnk_otid) -> caf::result<std::string> {
				return delegate_source<std::string>(type_descriptor::nil().name, a_lnk_otid());
			},

			[this](a_node_gid) -> caf::result< result_or_errbox<std::string> > {
				return delegate_source< result_or_errbox<std::string> >(a_node_gid());
			},

			[this](a_lnk_inode) -> caf::result< result_or_errbox<inodeptr> > {
				return delegate_source< result_or_errbox<std::string> >(a_lnk_inode());
			},

		}).or_else(super::make_behavior());
	}
};

// impl
sym_link_impl::sym_link_impl(std::string name, std::string path, Flags f)
	: super(std::move(name), f), path_(std::move(path))
{}

sym_link_impl::sym_link_impl()
	: super()
{}

auto sym_link_impl::pointee() const -> result_or_err<sp_link> {
	const auto parent = owner_.lock();
	if(!parent) return tl::make_unexpected( error::quiet(Error::UnboundSymLink) );

	sp_link src_link;
	if(auto er = error::eval_safe([&] { src_link = deref_path(path_, parent); }); er)
		return tl::make_unexpected(std::move(er));
	else if(src_link)
		return src_link;
	return tl::make_unexpected( error::quiet(Error::LinkExpired) );
}

auto sym_link_impl::data() -> result_or_err<sp_obj> {
	auto res = pointee().and_then([](const sp_link& src_link) {
		return src_link->data_ex();
	});
#if defined(_DEBUG)
	if(!res) {
		auto& er = res.error();
		std::cout << ">>> " << to_string(id_) << ' ' << er.what() << std::endl;
		//std::cout << kernel::tools::get_backtrace(16) << std::endl;
	}
#endif
	return res;
}

auto sym_link_impl::spawn_actor(sp_limpl limpl) const -> caf::actor {
	return spawn_lactor<sym_link_actor>(std::move(limpl));
}

///////////////////////////////////////////////////////////////////////////////
//  class
//
/// ctor -- pointee is specified by string path
sym_link::sym_link(std::string name, std::string path, Flags f)
	: link(std::make_shared<sym_link_impl>(std::move(name), std::move(path), f))
{}
/// ctor -- pointee is specified directly - absolute path will be stored
sym_link::sym_link(std::string name, const sp_link& src, Flags f)
	: sym_link(std::move(name), abspath(src), f)
{}

sym_link::sym_link()
	: super(std::make_shared<sym_link_impl>(), false)
{}

/// implement link's API
sp_link sym_link::clone(bool deep) const {
	// no deep copy support for symbolic link
	return std::make_shared<sym_link>(pimpl()->name_, pimpl()->path_, flags());
}

std::string sym_link::type_id() const {
	return "sym_link";
}

auto sym_link::pimpl() const -> sym_link_impl* {
	return static_cast<sym_link_impl*>(super::pimpl());
}

bool sym_link::check_alive() {
	auto res = bool(deref_path(pimpl()->path_, owner()));
	auto S = res ? ReqStatus::OK : ReqStatus::Error;
	rs_reset_if_neq(Req::Data, S, S);
	return res;
}

/// return stored pointee path
std::string sym_link::src_path(bool human_readable) const {
	if(!human_readable) return pimpl()->path_;
	else if(const auto parent = owner())
		return convert_path(pimpl()->path_, parent->handle());
	return {};
}

result_or_err<sp_node> sym_link::propagate_handle() {
	// sym link cannot be a node's handle
	return data_node_ex();
}

NAMESPACE_END(blue_sky::tree)
<commit_msg>tree/sym_link: optimize delegatin messages to src link, fix inode getter<commit_after>/// @file
/// @author uentity
/// @date 20.11.2017
/// @brief Symbolic link implementation
/// @copyright
/// This Source Code Form is subject to the terms of the Mozilla Public License,
/// v. 2.0. If a copy of the MPL was not distributed with this file,
/// You can obtain one at https://mozilla.org/MPL/2.0/

#include <bs/log.h>
#include <bs/tree/tree.h>
#include <bs/tree/errors.h>
#include <bs/kernel/config.h>
#include <bs/kernel/tools.h>

#include <bs/serialize/tree.h>
#include <bs/serialize/cafbind.h>

#include "link_actor.h"

OMIT_OBJ_SERIALIZATION

NAMESPACE_BEGIN(blue_sky::tree)
///////////////////////////////////////////////////////////////////////////////
//  actor + impl
//

// actor
struct sym_link_actor : public link_actor {
	using super = link_actor;

	using super::super;

	auto pointee() const -> result_or_err<sp_link> {
		return static_cast<sym_link_impl&>(impl).pointee();
	}

	//// make data request via source link
	//auto data_ex(obj_processor_f cb, ReqOpts opts) -> void override {
	//	pointee().and_then([&](const sp_link& src_link) {
	//		if(enumval(opts & ReqOpts::DirectInvoke))
	//			cb(src_link->data_ex());
	//		else
	//			caf::anon_send(src_link->actor(), a_lnk_data(), std::move(cb), opts);
	//	});
	//}

	//auto data_node_ex(node_processor_f cb, ReqOpts opts) -> void override {
	//	pointee().and_then([&](const sp_link& src_link) {
	//		if(enumval(opts & ReqOpts::DirectInvoke))
	//			cb(src_link->data_node_ex());
	//		else
	//			caf::anon_send(src_link->actor(), a_lnk_dnode(), std::move(cb), opts);
	//	});
	//}

	template<typename R, typename... Args>
	auto delegate_source(Args&&... args)
	-> std::enable_if_t<tl::detail::is_expected<R>::value, caf::result<R>> {
		if(auto p = pointee())
			return delegate(link::actor(*p.value()), std::forward<Args>(args)...);
		else
			return R{ tl::unexpect, p.error() };
	}

	template<typename R, typename... Args>
	auto delegate_source(R errval, Args&&... args)
	-> std::enable_if_t<!tl::detail::is_expected<R>::value, caf::result<R>> {
		if(auto p = pointee())
			return delegate(link::actor(*p.value()), std::forward<Args>(args)...);
		else
			return errval;
	}

	// delegate name, OID, etc requests to source link
	auto make_behavior() -> caf::behavior override {
		return caf::message_handler({

			[this](a_lnk_oid) -> caf::result<std::string> {
				return delegate_source<std::string>(type_descriptor::nil().name, a_lnk_oid());
			},

			[this](a_lnk_otid) -> caf::result<std::string> {
				return delegate_source<std::string>(type_descriptor::nil().name, a_lnk_otid());
			},

			[this](a_node_gid) -> caf::result< result_or_errbox<std::string> > {
				return delegate_source< result_or_errbox<std::string> >(a_node_gid());
			},

			[this](a_lnk_inode) -> caf::result< result_or_errbox<inodeptr> > {
				return delegate_source< result_or_errbox<inodeptr> >(a_lnk_inode());
			},

		}).or_else(super::make_behavior());
	}
};

// impl
sym_link_impl::sym_link_impl(std::string name, std::string path, Flags f)
	: super(std::move(name), f), path_(std::move(path))
{}

sym_link_impl::sym_link_impl()
	: super()
{}

auto sym_link_impl::pointee() const -> result_or_err<sp_link> {
	const auto parent = owner_.lock();
	if(!parent) return tl::make_unexpected( error::quiet(Error::UnboundSymLink) );

	sp_link src_link;
	if(auto er = error::eval_safe([&] { src_link = deref_path(path_, parent); }); er)
		return tl::make_unexpected(std::move(er));
	else if(src_link)
		return src_link;
	return tl::make_unexpected( error::quiet(Error::LinkExpired) );
}

auto sym_link_impl::data() -> result_or_err<sp_obj> {
	auto res = pointee().and_then([](const sp_link& src_link) {
		return src_link->data_ex();
	});
#if defined(_DEBUG)
	if(!res) {
		auto& er = res.error();
		std::cout << ">>> " << to_string(id_) << ' ' << er.what() << std::endl;
		//std::cout << kernel::tools::get_backtrace(16) << std::endl;
	}
#endif
	return res;
}

auto sym_link_impl::spawn_actor(sp_limpl limpl) const -> caf::actor {
	return spawn_lactor<sym_link_actor>(std::move(limpl));
}

///////////////////////////////////////////////////////////////////////////////
//  class
//
/// ctor -- pointee is specified by string path
sym_link::sym_link(std::string name, std::string path, Flags f)
	: link(std::make_shared<sym_link_impl>(std::move(name), std::move(path), f))
{}
/// ctor -- pointee is specified directly - absolute path will be stored
sym_link::sym_link(std::string name, const sp_link& src, Flags f)
	: sym_link(std::move(name), abspath(src), f)
{}

sym_link::sym_link()
	: super(std::make_shared<sym_link_impl>(), false)
{}

/// implement link's API
sp_link sym_link::clone(bool deep) const {
	// no deep copy support for symbolic link
	return std::make_shared<sym_link>(pimpl()->name_, pimpl()->path_, flags());
}

std::string sym_link::type_id() const {
	return "sym_link";
}

auto sym_link::pimpl() const -> sym_link_impl* {
	return static_cast<sym_link_impl*>(super::pimpl());
}

bool sym_link::check_alive() {
	auto res = bool(deref_path(pimpl()->path_, owner()));
	auto S = res ? ReqStatus::OK : ReqStatus::Error;
	rs_reset_if_neq(Req::Data, S, S);
	return res;
}

/// return stored pointee path
std::string sym_link::src_path(bool human_readable) const {
	if(!human_readable) return pimpl()->path_;
	else if(const auto parent = owner())
		return convert_path(pimpl()->path_, parent->handle());
	return {};
}

auto sym_link::propagate_handle() -> result_or_err<sp_node> {
	// sym link cannot be a node's handle
	return data_node_ex();
}

NAMESPACE_END(blue_sky::tree)
<|endoftext|>


<commit_before>//===-- BenchmarkResult.cpp -------------------------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "BenchmarkResult.h"
#include "llvm/ADT/STLExtras.h"
#include "llvm/ADT/StringRef.h"
#include "llvm/ObjectYAML/YAML.h"
#include "llvm/Support/FileOutputBuffer.h"
#include "llvm/Support/FileSystem.h"
#include "llvm/Support/Format.h"
#include "llvm/Support/raw_ostream.h"

static constexpr const char kIntegerFormat[] = "i_0x%" PRId64 "x";
static constexpr const char kDoubleFormat[] = "f_%la";

static void serialize(const exegesis::BenchmarkResultContext &Context,
                      const llvm::MCOperand &MCOperand, llvm::raw_ostream &OS) {
  if (MCOperand.isReg()) {
    OS << Context.getRegName(MCOperand.getReg());
  } else if (MCOperand.isImm()) {
    OS << llvm::format(kIntegerFormat, MCOperand.getImm());
  } else if (MCOperand.isFPImm()) {
    OS << llvm::format(kDoubleFormat, MCOperand.getFPImm());
  } else {
    OS << "INVALID";
  }
}

static void serialize(const exegesis::BenchmarkResultContext &Context,
                      const llvm::MCInst &MCInst, llvm::raw_ostream &OS) {
  OS << Context.getInstrName(MCInst.getOpcode());
  for (const auto &Op : MCInst) {
    OS << ' ';
    serialize(Context, Op, OS);
  }
}

static llvm::MCOperand
deserialize(const exegesis::BenchmarkResultContext &Context,
            llvm::StringRef String) {
  assert(!String.empty());
  int64_t IntValue = 0;
  double DoubleValue = 0;
  if (sscanf(String.data(), kIntegerFormat, &IntValue) == 1)
    return llvm::MCOperand::createImm(IntValue);
  if (sscanf(String.data(), kDoubleFormat, &DoubleValue) == 1)
    return llvm::MCOperand::createFPImm(DoubleValue);
  if (unsigned RegNo = Context.getRegNo(String)) // Returns 0 if invalid.
    return llvm::MCOperand::createReg(RegNo);
  return {};
}

static llvm::StringRef
deserialize(const exegesis::BenchmarkResultContext &Context,
            llvm::StringRef String, llvm::MCInst &Value) {
  llvm::SmallVector<llvm::StringRef, 8> Pieces;
  String.split(Pieces, " ");
  if (Pieces.empty())
    return "Invalid Instruction";
  bool ProcessOpcode = true;
  for (llvm::StringRef Piece : Pieces) {
    if (ProcessOpcode) {
      ProcessOpcode = false;
      Value.setOpcode(Context.getInstrOpcode(Piece));
      if (Value.getOpcode() == 0)
        return "Unknown Opcode Name";
    } else {
      Value.addOperand(deserialize(Context, Piece));
    }
  }
  return {};
}

// YAML IO requires a mutable pointer to Context but we guarantee to not
// modify it.
static void *getUntypedContext(const exegesis::BenchmarkResultContext &Ctx) {
  return const_cast<exegesis::BenchmarkResultContext *>(&Ctx);
}

static const exegesis::BenchmarkResultContext &getTypedContext(void *Ctx) {
  assert(Ctx);
  return *static_cast<const exegesis::BenchmarkResultContext *>(Ctx);
}

// Defining YAML traits for IO.
namespace llvm {
namespace yaml {

// std::vector<llvm::MCInst> will be rendered as a list.
template <> struct SequenceElementTraits<llvm::MCInst> {
  static const bool flow = false;
};

template <> struct ScalarTraits<llvm::MCInst> {

  static void output(const llvm::MCInst &Value, void *Ctx,
                     llvm::raw_ostream &Out) {
    serialize(getTypedContext(Ctx), Value, Out);
  }

  static StringRef input(StringRef Scalar, void *Ctx, llvm::MCInst &Value) {
    return deserialize(getTypedContext(Ctx), Scalar, Value);
  }

  static QuotingType mustQuote(StringRef) { return QuotingType::Single; }

  static const bool flow = true;
};

// std::vector<exegesis::Measure> will be rendered as a list.
template <> struct SequenceElementTraits<exegesis::BenchmarkMeasure> {
  static const bool flow = false;
};

// exegesis::Measure is rendererd as a flow instead of a list.
// e.g. { "key": "the key", "value": 0123 }
template <> struct MappingTraits<exegesis::BenchmarkMeasure> {
  static void mapping(IO &Io, exegesis::BenchmarkMeasure &Obj) {
    Io.mapRequired("key", Obj.Key);
    Io.mapRequired("value", Obj.Value);
    Io.mapOptional("debug_string", Obj.DebugString);
  }
  static const bool flow = true;
};

template <>
struct ScalarEnumerationTraits<exegesis::InstructionBenchmark::ModeE> {
  static void enumeration(IO &Io,
                          exegesis::InstructionBenchmark::ModeE &Value) {
    Io.enumCase(Value, "", exegesis::InstructionBenchmark::Unknown);
    Io.enumCase(Value, "latency", exegesis::InstructionBenchmark::Latency);
    Io.enumCase(Value, "uops", exegesis::InstructionBenchmark::Uops);
  }
};

template <> struct MappingTraits<exegesis::InstructionBenchmarkKey> {
  static void mapping(IO &Io, exegesis::InstructionBenchmarkKey &Obj) {
    Io.mapRequired("instructions", Obj.Instructions);
    Io.mapOptional("config", Obj.Config);
  }
};

template <> struct MappingTraits<exegesis::InstructionBenchmark> {
  class NormalizedBinary {
  public:
    NormalizedBinary(IO &io) {}
    NormalizedBinary(IO &, std::vector<uint8_t> &Data) : Binary(Data) {}
    std::vector<uint8_t> denormalize(IO &) {
      std::vector<uint8_t> Data;
      std::string Str;
      raw_string_ostream OSS(Str);
      Binary.writeAsBinary(OSS);
      OSS.flush();
      Data.assign(Str.begin(), Str.end());
      return Data;
    }

    BinaryRef Binary;
  };

  static void mapping(IO &Io, exegesis::InstructionBenchmark &Obj) {
    Io.mapRequired("mode", Obj.Mode);
    Io.mapRequired("key", Obj.Key);
    Io.mapRequired("cpu_name", Obj.CpuName);
    Io.mapRequired("llvm_triple", Obj.LLVMTriple);
    Io.mapRequired("num_repetitions", Obj.NumRepetitions);
    Io.mapRequired("measurements", Obj.Measurements);
    Io.mapRequired("error", Obj.Error);
    Io.mapOptional("info", Obj.Info);
    // AssembledSnippet
    MappingNormalization<NormalizedBinary, std::vector<uint8_t>> BinaryString(
        Io, Obj.AssembledSnippet);
    Io.mapOptional("assembled_snippet", BinaryString->Binary);
  }
};

} // namespace yaml
} // namespace llvm

LLVM_YAML_IS_DOCUMENT_LIST_VECTOR(exegesis::InstructionBenchmark)

namespace exegesis {

void BenchmarkResultContext::addRegEntry(unsigned RegNo, llvm::StringRef Name) {
  assert(RegNoToName.find(RegNo) == RegNoToName.end());
  assert(RegNameToNo.find(Name) == RegNameToNo.end());
  RegNoToName[RegNo] = Name;
  RegNameToNo[Name] = RegNo;
}

llvm::StringRef BenchmarkResultContext::getRegName(unsigned RegNo) const {
  const auto Itr = RegNoToName.find(RegNo);
  if (Itr != RegNoToName.end())
    return Itr->second;
  return {};
}

unsigned BenchmarkResultContext::getRegNo(llvm::StringRef Name) const {
  const auto Itr = RegNameToNo.find(Name);
  if (Itr != RegNameToNo.end())
    return Itr->second;
  return 0;
}

void BenchmarkResultContext::addInstrEntry(unsigned Opcode,
                                           llvm::StringRef Name) {
  assert(InstrOpcodeToName.find(Opcode) == InstrOpcodeToName.end());
  assert(InstrNameToOpcode.find(Name) == InstrNameToOpcode.end());
  InstrOpcodeToName[Opcode] = Name;
  InstrNameToOpcode[Name] = Opcode;
}

llvm::StringRef BenchmarkResultContext::getInstrName(unsigned Opcode) const {
  const auto Itr = InstrOpcodeToName.find(Opcode);
  if (Itr != InstrOpcodeToName.end())
    return Itr->second;
  return {};
}

unsigned BenchmarkResultContext::getInstrOpcode(llvm::StringRef Name) const {
  const auto Itr = InstrNameToOpcode.find(Name);
  if (Itr != InstrNameToOpcode.end())
    return Itr->second;
  return 0;
}

template <typename ObjectOrList>
static llvm::Expected<ObjectOrList>
readYamlCommon(const BenchmarkResultContext &Context,
               llvm::StringRef Filename) {
  if (auto ExpectedMemoryBuffer =
          llvm::errorOrToExpected(llvm::MemoryBuffer::getFile(Filename))) {
    std::unique_ptr<llvm::MemoryBuffer> MemoryBuffer =
        std::move(ExpectedMemoryBuffer.get());
    llvm::yaml::Input Yin(*MemoryBuffer, getUntypedContext(Context));
    ObjectOrList Benchmark;
    Yin >> Benchmark;
    return Benchmark;
  } else {
    return ExpectedMemoryBuffer.takeError();
  }
}

llvm::Expected<InstructionBenchmark>
InstructionBenchmark::readYaml(const BenchmarkResultContext &Context,
                               llvm::StringRef Filename) {
  return readYamlCommon<InstructionBenchmark>(Context, Filename);
}

llvm::Expected<std::vector<InstructionBenchmark>>
InstructionBenchmark::readYamls(const BenchmarkResultContext &Context,
                                llvm::StringRef Filename) {
  return readYamlCommon<std::vector<InstructionBenchmark>>(Context, Filename);
}

void InstructionBenchmark::writeYamlTo(const BenchmarkResultContext &Context,
                                       llvm::raw_ostream &OS) {
  llvm::yaml::Output Yout(OS, getUntypedContext(Context));
  Yout << *this;
}

void InstructionBenchmark::readYamlFrom(const BenchmarkResultContext &Context,
                                        llvm::StringRef InputContent) {
  llvm::yaml::Input Yin(InputContent, getUntypedContext(Context));
  Yin >> *this;
}

llvm::Error
InstructionBenchmark::writeYaml(const BenchmarkResultContext &Context,
                                const llvm::StringRef Filename) {
  if (Filename == "-") {
    writeYamlTo(Context, llvm::outs());
  } else {
    int ResultFD = 0;
    if (auto E = llvm::errorCodeToError(
            openFileForWrite(Filename, ResultFD, llvm::sys::fs::CD_CreateAlways,
                             llvm::sys::fs::F_Text))) {
      return E;
    }
    llvm::raw_fd_ostream Ostr(ResultFD, true /*shouldClose*/);
    writeYamlTo(Context, Ostr);
  }
  return llvm::Error::success();
}

void BenchmarkMeasureStats::push(const BenchmarkMeasure &BM) {
  if (Key.empty())
    Key = BM.Key;
  assert(Key == BM.Key);
  ++NumValues;
  SumValues += BM.Value;
  MaxValue = std::max(MaxValue, BM.Value);
  MinValue = std::min(MinValue, BM.Value);
}

} // namespace exegesis
<commit_msg>[llvm-exegesis] Ignore double spaced separators in asm strings<commit_after>//===-- BenchmarkResult.cpp -------------------------------------*- C++ -*-===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "BenchmarkResult.h"
#include "llvm/ADT/STLExtras.h"
#include "llvm/ADT/StringRef.h"
#include "llvm/ObjectYAML/YAML.h"
#include "llvm/Support/FileOutputBuffer.h"
#include "llvm/Support/FileSystem.h"
#include "llvm/Support/Format.h"
#include "llvm/Support/raw_ostream.h"

static constexpr const char kIntegerFormat[] = "i_0x%" PRId64 "x";
static constexpr const char kDoubleFormat[] = "f_%la";

static void serialize(const exegesis::BenchmarkResultContext &Context,
                      const llvm::MCOperand &MCOperand, llvm::raw_ostream &OS) {
  if (MCOperand.isReg()) {
    OS << Context.getRegName(MCOperand.getReg());
  } else if (MCOperand.isImm()) {
    OS << llvm::format(kIntegerFormat, MCOperand.getImm());
  } else if (MCOperand.isFPImm()) {
    OS << llvm::format(kDoubleFormat, MCOperand.getFPImm());
  } else {
    OS << "INVALID";
  }
}

static void serialize(const exegesis::BenchmarkResultContext &Context,
                      const llvm::MCInst &MCInst, llvm::raw_ostream &OS) {
  OS << Context.getInstrName(MCInst.getOpcode());
  for (const auto &Op : MCInst) {
    OS << ' ';
    serialize(Context, Op, OS);
  }
}

static llvm::MCOperand
deserialize(const exegesis::BenchmarkResultContext &Context,
            llvm::StringRef String) {
  assert(!String.empty());
  int64_t IntValue = 0;
  double DoubleValue = 0;
  if (sscanf(String.data(), kIntegerFormat, &IntValue) == 1)
    return llvm::MCOperand::createImm(IntValue);
  if (sscanf(String.data(), kDoubleFormat, &DoubleValue) == 1)
    return llvm::MCOperand::createFPImm(DoubleValue);
  if (unsigned RegNo = Context.getRegNo(String)) // Returns 0 if invalid.
    return llvm::MCOperand::createReg(RegNo);
  return {};
}

static llvm::StringRef
deserialize(const exegesis::BenchmarkResultContext &Context,
            llvm::StringRef String, llvm::MCInst &Value) {
  llvm::SmallVector<llvm::StringRef, 8> Pieces;
  String.split(Pieces, " ", /* MaxSplit */ -1, /* KeepEmpty */ false);
  if (Pieces.empty())
    return "Invalid Instruction";
  bool ProcessOpcode = true;
  for (llvm::StringRef Piece : Pieces) {
    if (ProcessOpcode) {
      ProcessOpcode = false;
      Value.setOpcode(Context.getInstrOpcode(Piece));
      if (Value.getOpcode() == 0)
        return "Unknown Opcode Name";
    } else {
      Value.addOperand(deserialize(Context, Piece));
    }
  }
  return {};
}

// YAML IO requires a mutable pointer to Context but we guarantee to not
// modify it.
static void *getUntypedContext(const exegesis::BenchmarkResultContext &Ctx) {
  return const_cast<exegesis::BenchmarkResultContext *>(&Ctx);
}

static const exegesis::BenchmarkResultContext &getTypedContext(void *Ctx) {
  assert(Ctx);
  return *static_cast<const exegesis::BenchmarkResultContext *>(Ctx);
}

// Defining YAML traits for IO.
namespace llvm {
namespace yaml {

// std::vector<llvm::MCInst> will be rendered as a list.
template <> struct SequenceElementTraits<llvm::MCInst> {
  static const bool flow = false;
};

template <> struct ScalarTraits<llvm::MCInst> {

  static void output(const llvm::MCInst &Value, void *Ctx,
                     llvm::raw_ostream &Out) {
    serialize(getTypedContext(Ctx), Value, Out);
  }

  static StringRef input(StringRef Scalar, void *Ctx, llvm::MCInst &Value) {
    return deserialize(getTypedContext(Ctx), Scalar, Value);
  }

  static QuotingType mustQuote(StringRef) { return QuotingType::Single; }

  static const bool flow = true;
};

// std::vector<exegesis::Measure> will be rendered as a list.
template <> struct SequenceElementTraits<exegesis::BenchmarkMeasure> {
  static const bool flow = false;
};

// exegesis::Measure is rendererd as a flow instead of a list.
// e.g. { "key": "the key", "value": 0123 }
template <> struct MappingTraits<exegesis::BenchmarkMeasure> {
  static void mapping(IO &Io, exegesis::BenchmarkMeasure &Obj) {
    Io.mapRequired("key", Obj.Key);
    Io.mapRequired("value", Obj.Value);
    Io.mapOptional("debug_string", Obj.DebugString);
  }
  static const bool flow = true;
};

template <>
struct ScalarEnumerationTraits<exegesis::InstructionBenchmark::ModeE> {
  static void enumeration(IO &Io,
                          exegesis::InstructionBenchmark::ModeE &Value) {
    Io.enumCase(Value, "", exegesis::InstructionBenchmark::Unknown);
    Io.enumCase(Value, "latency", exegesis::InstructionBenchmark::Latency);
    Io.enumCase(Value, "uops", exegesis::InstructionBenchmark::Uops);
  }
};

template <> struct MappingTraits<exegesis::InstructionBenchmarkKey> {
  static void mapping(IO &Io, exegesis::InstructionBenchmarkKey &Obj) {
    Io.mapRequired("instructions", Obj.Instructions);
    Io.mapOptional("config", Obj.Config);
  }
};

template <> struct MappingTraits<exegesis::InstructionBenchmark> {
  class NormalizedBinary {
  public:
    NormalizedBinary(IO &io) {}
    NormalizedBinary(IO &, std::vector<uint8_t> &Data) : Binary(Data) {}
    std::vector<uint8_t> denormalize(IO &) {
      std::vector<uint8_t> Data;
      std::string Str;
      raw_string_ostream OSS(Str);
      Binary.writeAsBinary(OSS);
      OSS.flush();
      Data.assign(Str.begin(), Str.end());
      return Data;
    }

    BinaryRef Binary;
  };

  static void mapping(IO &Io, exegesis::InstructionBenchmark &Obj) {
    Io.mapRequired("mode", Obj.Mode);
    Io.mapRequired("key", Obj.Key);
    Io.mapRequired("cpu_name", Obj.CpuName);
    Io.mapRequired("llvm_triple", Obj.LLVMTriple);
    Io.mapRequired("num_repetitions", Obj.NumRepetitions);
    Io.mapRequired("measurements", Obj.Measurements);
    Io.mapRequired("error", Obj.Error);
    Io.mapOptional("info", Obj.Info);
    // AssembledSnippet
    MappingNormalization<NormalizedBinary, std::vector<uint8_t>> BinaryString(
        Io, Obj.AssembledSnippet);
    Io.mapOptional("assembled_snippet", BinaryString->Binary);
  }
};

} // namespace yaml
} // namespace llvm

LLVM_YAML_IS_DOCUMENT_LIST_VECTOR(exegesis::InstructionBenchmark)

namespace exegesis {

void BenchmarkResultContext::addRegEntry(unsigned RegNo, llvm::StringRef Name) {
  assert(RegNoToName.find(RegNo) == RegNoToName.end());
  assert(RegNameToNo.find(Name) == RegNameToNo.end());
  RegNoToName[RegNo] = Name;
  RegNameToNo[Name] = RegNo;
}

llvm::StringRef BenchmarkResultContext::getRegName(unsigned RegNo) const {
  const auto Itr = RegNoToName.find(RegNo);
  if (Itr != RegNoToName.end())
    return Itr->second;
  return {};
}

unsigned BenchmarkResultContext::getRegNo(llvm::StringRef Name) const {
  const auto Itr = RegNameToNo.find(Name);
  if (Itr != RegNameToNo.end())
    return Itr->second;
  return 0;
}

void BenchmarkResultContext::addInstrEntry(unsigned Opcode,
                                           llvm::StringRef Name) {
  assert(InstrOpcodeToName.find(Opcode) == InstrOpcodeToName.end());
  assert(InstrNameToOpcode.find(Name) == InstrNameToOpcode.end());
  InstrOpcodeToName[Opcode] = Name;
  InstrNameToOpcode[Name] = Opcode;
}

llvm::StringRef BenchmarkResultContext::getInstrName(unsigned Opcode) const {
  const auto Itr = InstrOpcodeToName.find(Opcode);
  if (Itr != InstrOpcodeToName.end())
    return Itr->second;
  return {};
}

unsigned BenchmarkResultContext::getInstrOpcode(llvm::StringRef Name) const {
  const auto Itr = InstrNameToOpcode.find(Name);
  if (Itr != InstrNameToOpcode.end())
    return Itr->second;
  return 0;
}

template <typename ObjectOrList>
static llvm::Expected<ObjectOrList>
readYamlCommon(const BenchmarkResultContext &Context,
               llvm::StringRef Filename) {
  if (auto ExpectedMemoryBuffer =
          llvm::errorOrToExpected(llvm::MemoryBuffer::getFile(Filename))) {
    std::unique_ptr<llvm::MemoryBuffer> MemoryBuffer =
        std::move(ExpectedMemoryBuffer.get());
    llvm::yaml::Input Yin(*MemoryBuffer, getUntypedContext(Context));
    ObjectOrList Benchmark;
    Yin >> Benchmark;
    return Benchmark;
  } else {
    return ExpectedMemoryBuffer.takeError();
  }
}

llvm::Expected<InstructionBenchmark>
InstructionBenchmark::readYaml(const BenchmarkResultContext &Context,
                               llvm::StringRef Filename) {
  return readYamlCommon<InstructionBenchmark>(Context, Filename);
}

llvm::Expected<std::vector<InstructionBenchmark>>
InstructionBenchmark::readYamls(const BenchmarkResultContext &Context,
                                llvm::StringRef Filename) {
  return readYamlCommon<std::vector<InstructionBenchmark>>(Context, Filename);
}

void InstructionBenchmark::writeYamlTo(const BenchmarkResultContext &Context,
                                       llvm::raw_ostream &OS) {
  llvm::yaml::Output Yout(OS, getUntypedContext(Context));
  Yout << *this;
}

void InstructionBenchmark::readYamlFrom(const BenchmarkResultContext &Context,
                                        llvm::StringRef InputContent) {
  llvm::yaml::Input Yin(InputContent, getUntypedContext(Context));
  Yin >> *this;
}

llvm::Error
InstructionBenchmark::writeYaml(const BenchmarkResultContext &Context,
                                const llvm::StringRef Filename) {
  if (Filename == "-") {
    writeYamlTo(Context, llvm::outs());
  } else {
    int ResultFD = 0;
    if (auto E = llvm::errorCodeToError(
            openFileForWrite(Filename, ResultFD, llvm::sys::fs::CD_CreateAlways,
                             llvm::sys::fs::F_Text))) {
      return E;
    }
    llvm::raw_fd_ostream Ostr(ResultFD, true /*shouldClose*/);
    writeYamlTo(Context, Ostr);
  }
  return llvm::Error::success();
}

void BenchmarkMeasureStats::push(const BenchmarkMeasure &BM) {
  if (Key.empty())
    Key = BM.Key;
  assert(Key == BM.Key);
  ++NumValues;
  SumValues += BM.Value;
  MaxValue = std::max(MaxValue, BM.Value);
  MinValue = std::min(MinValue, BM.Value);
}

} // namespace exegesis
<|endoftext|>


<commit_before>/*
* Copyright (C) 2008-2011 J-P Nurmi <jpnurmi@gmail.com>
*
* This library 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 2 of the License, or (at your
* option) any later version.
*
* This library 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 Lesser General Public
* License for more details.
*/

#include "ircencoder_p.h"
#include <QTextCodec>

#ifdef HAVE_ICU
#include <unicode/ucsdet.h>
#endif // HAVE_ICU

IrcEncoder::IrcEncoder()
{
    d.detector = 0;
#ifdef HAVE_ICU
    UErrorCode status = U_ZERO_ERROR;
    d.detector = ucsdet_open(&status);
    if (U_FAILURE(status))
        qWarning("IrcEncoder: ICU initialization failed: %s", u_errorName(status));
#endif // HAVE_ICU
}

IrcEncoder::~IrcEncoder()
{
#ifdef HAVE_ICU
    ucsdet_close(d.detector);
#endif // HAVE_ICU
}

QString IrcEncoder::encode(const QByteArray& data) const
{
    QByteArray enc = d.encoding;
    if (enc.isNull())
        enc = detectEncoding(data);
    QTextCodec *codec = QTextCodec::codecForName(d.encoding);
    if (!codec)
        codec = QTextCodec::codecForLocale();
    return codec->toUnicode(data);
}

QByteArray IrcEncoder::detectEncoding(const QByteArray& data) const
{
    Q_UNUSED(data);
    QByteArray encoding;
#ifdef HAVE_ICU
    UErrorCode status = U_ZERO_ERROR;
    if (d.detector)
    {
        ucsdet_setText(d.detector, data.constData(), data.length(), &status);
        if (!U_FAILURE(status))
        {
            const UCharsetMatch* match = ucsdet_detect(d.detector, &status);
            if (match && !U_FAILURE(status))
                encoding = ucsdet_getName(match, &status);
        }
    }
    if (U_FAILURE(status))
        qWarning("IrcEncoder::detectEncoding() failed: %s", u_errorName(status));
#endif // HAVE_ICU
    return encoding;
}
<commit_msg>IrcEncoder: revised encoding auto-detection<commit_after>/*
* Copyright (C) 2008-2011 J-P Nurmi <jpnurmi@gmail.com>
*
* This library 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 2 of the License, or (at your
* option) any later version.
*
* This library 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 Lesser General Public
* License for more details.
*/

#include "ircencoder_p.h"
#include <QTextCodec>

#ifdef HAVE_ICU
#include <unicode/ucsdet.h>
#endif // HAVE_ICU

static bool isUtf8(const QByteArray& utf8);

IrcEncoder::IrcEncoder()
{
    d.detector = 0;
#ifdef HAVE_ICU
    UErrorCode status = U_ZERO_ERROR;
    d.detector = ucsdet_open(&status);
    if (U_FAILURE(status))
        qWarning("IrcEncoder: ICU initialization failed: %s", u_errorName(status));
#endif // HAVE_ICU
}

IrcEncoder::~IrcEncoder()
{
#ifdef HAVE_ICU
    ucsdet_close(d.detector);
#endif // HAVE_ICU
}

QString IrcEncoder::encode(const QByteArray& data) const
{
    QByteArray enc = d.encoding;
    if (enc.isNull())
        enc = detectEncoding(data);
    QTextCodec *codec = QTextCodec::codecForName(enc);
    if (!codec)
        codec = QTextCodec::codecForLocale();
    return codec->toUnicode(data);
}

QByteArray IrcEncoder::detectEncoding(const QByteArray& data) const
{
    QByteArray encoding("ISO 8859-1");
    if (isUtf8(data))
        return "UTF-8";
#ifdef HAVE_ICU
    UErrorCode status = U_ZERO_ERROR;
    if (d.detector)
    {
        ucsdet_setText(d.detector, data.constData(), data.length(), &status);
        if (!U_FAILURE(status))
        {
            const UCharsetMatch* match = ucsdet_detect(d.detector, &status);
            if (match && !U_FAILURE(status))
                encoding = ucsdet_getName(match, &status);
        }
    }
    if (U_FAILURE(status))
        qWarning("IrcEncoder::detectEncoding() failed: %s", u_errorName(status));
#endif // HAVE_ICU
    return encoding;
}

/*
  The Original Code is mozilla.org code.
  See http://lxr.mozilla.org/mozilla/source/modules/rdf/src/utils.c

  Copyright (C) 1998 Netscape Communications Corporation
*/

#define kLeft1BitMask  0x80
#define kLeft2BitsMask 0xC0
#define kLeft3BitsMask 0xE0
#define kLeft4BitsMask 0xF0
#define kLeft5BitsMask 0xF8
#define kLeft6BitsMask 0xFC
#define kLeft7BitsMask 0xFE

#define k2BytesLeadByte kLeft2BitsMask
#define k3BytesLeadByte kLeft3BitsMask
#define k4BytesLeadByte kLeft4BitsMask
#define k5BytesLeadByte kLeft5BitsMask
#define k6BytesLeadByte kLeft6BitsMask
#define kTrialByte      kLeft1BitMask

#define UTF8_1Byte(c) ( 0 == ((c) & kLeft1BitMask))
#define UTF8_2Bytes(c) ( k2BytesLeadByte == ((c) & kLeft3BitsMask))
#define UTF8_3Bytes(c) ( k3BytesLeadByte == ((c) & kLeft4BitsMask))
#define UTF8_4Bytes(c) ( k4BytesLeadByte == ((c) & kLeft5BitsMask))
#define UTF8_5Bytes(c) ( k5BytesLeadByte == ((c) & kLeft6BitsMask))
#define UTF8_6Bytes(c) ( k6BytesLeadByte == ((c) & kLeft7BitsMask))
#define UTF8_ValidTrialByte(c) ( kTrialByte == ((c) & kLeft2BitsMask))

bool isUtf8(const QByteArray& utf8)
{
    int clen = 0;
    for (int i = 0; i < utf8.length(); i += clen)
    {
        if (UTF8_1Byte(utf8[i]))
        {
            clen = 1;
        }
        else if (UTF8_2Bytes(utf8[i]))
        {
            clen = 2;
            // No enough trail bytes
            if ((i + clen) > utf8.length())
                return false;
            // 0000 0000 - 0000 007F : should encode in less bytes
            if (0 ==  (utf8[i] & 0x1E))
                return false;
        }
        else if (UTF8_3Bytes(utf8[i]))
        {
            clen = 3;
            // No enough trail bytes
            if ((i + clen) > utf8.length())
                return false;
            // a single Surrogate should not show in 3 bytes UTF8, instead,
            // the pair should be intepreted as one single UCS4 char and
            // encoded UTF8 in 4 bytes
            if ((0xED == utf8[i]) && (0xA0 == (utf8[i+1] & 0xA0)))
                return false;
            // 0000 0000 - 0000 07FF : should encode in less bytes
            if ((0 == (utf8[i] & 0x0F)) && (0 == (utf8[i+1] & 0x20)))
                return false;
        }
        else if (UTF8_4Bytes(utf8[i]))
        {
            clen = 4;
            // No enough trail bytes
            if ((i + clen) > utf8.length())
                return false;
            // 0000 0000 - 0000 FFFF : should encode in less bytes
            if ((0 == (utf8[i] & 0x07 )) && (0 == (utf8[i+1] & 0x30)))
                return false;
        }
        else if (UTF8_5Bytes(utf8[i]))
        {
            clen = 5;
            // No enough trail bytes
            if ((i + clen) > utf8.length())
                return false;
            // 0000 0000 - 001F FFFF : should encode in less bytes
            if ((0 == (utf8[i] & 0x03 )) && (0 == (utf8[i+1] & 0x38)))
                return false;
        }
        else if (UTF8_6Bytes(utf8[i]))
        {
            clen = 6;
            // No enough trail bytes
            if ((i + clen) > utf8.length())
                return false;
            // 0000 0000 - 03FF FFFF : should encode in less bytes
            if ((0 == (utf8[i] & 0x01)) && (0 == (utf8[i+1] & 0x3E)))
                return false;
        }
        else
        {
            return false;
        }
        for (int j = 1; j < clen; ++j)
        {
            if (!UTF8_ValidTrialByte(utf8[i+j])) // Trail bytes invalid
                return false;
        }
    }
    return true;
}
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Wim Meeussen */

#include "urdf/link.h"
#include <ros/ros.h>
#include <boost/lexical_cast.hpp>

namespace urdf{

boost::shared_ptr<Geometry> parseGeometry(TiXmlElement *g)
{
  boost::shared_ptr<Geometry> geom;
  if (!g) return geom;

  TiXmlElement *shape = g->FirstChildElement();
  if (!shape)
  {
    ROS_ERROR("Geometry tag contains no child element.");
    return geom;
  }

  std::string type_name = shape->ValueStr();
  if (type_name == "sphere")
    geom.reset(new Sphere);
  else if (type_name == "box")
    geom.reset(new Box);
  else if (type_name == "cylinder")
    geom.reset(new Cylinder);
  else if (type_name == "mesh")
    geom.reset(new Mesh);
  else
  {
    ROS_ERROR("Unknown geometry type '%s'", type_name.c_str());
    return geom;
  }

  // clear geom object when fails to initialize
  if (!geom->initXml(shape)){
    ROS_ERROR("Geometry failed to parse");
    geom.reset();
  }

  return geom;
}

bool Material::initXml(TiXmlElement *config)
{
  bool has_rgb = false;
  bool has_filename = false;

  this->clear();

  if (!config->Attribute("name"))
  {
    ROS_ERROR("Material must contain a name attribute");
    return false;
  }

  this->name = config->Attribute("name");

  // texture
  TiXmlElement *t = config->FirstChildElement("texture");
  if (t)
  {
    if (t->Attribute("filename"))
    {
      this->texture_filename = t->Attribute("filename");
      has_filename = true;
    }
    else
    {
      ROS_ERROR("texture has no filename for Material %s",this->name.c_str());
    }
  }

  // color
  TiXmlElement *c = config->FirstChildElement("color");
  if (c)
  {
    if (c->Attribute("rgba"))
    {
      if (!this->color.init(c->Attribute("rgba")))
      {
        ROS_ERROR("Material %s has malformed color rgba values.",this->name.c_str());
        this->color.clear();
        return false;
      }
      else
        has_rgb = true;
    }
    else
    {
      ROS_ERROR("Material %s color has no rgba",this->name.c_str());
    }
  }

  return (has_rgb || has_filename);
}

bool Inertial::initXml(TiXmlElement *config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for inertial element, using default (Identity)");
    this->origin.clear();
  }
  else
  {
    if (!this->origin.initXml(o))
    {
      ROS_ERROR("Inertial has a malformed origin tag");
      this->origin.clear();
      return false;
    }
  }

  TiXmlElement *mass_xml = config->FirstChildElement("mass");
  if (!mass_xml)
  {
    ROS_ERROR("Inertial element must have mass element");
    return false;
  }
  if (!mass_xml->Attribute("value"))
  {
    ROS_ERROR("Inertial: mass element must have value attributes");
    return false;
  }

  try
  {
    mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("mass (%s) is not a float",mass_xml->Attribute("value"));
    return false;
  }

  TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
  if (!inertia_xml)
  {
    ROS_ERROR("Inertial element must have inertia element");
    return false;
  }
  if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
        inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
        inertia_xml->Attribute("izz")))
  {
    ROS_ERROR("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
    return false;
  }
  try
  {
    ixx  = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
    ixy  = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
    ixz  = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
    iyy  = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
    iyz  = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
    izz  = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("one of the inertia elements: ixx (%s) ixy (%s) ixz (%s) iyy (%s) iyz (%s) izz (%s) is not a valid double",
              inertia_xml->Attribute("ixx"),
              inertia_xml->Attribute("ixy"),
              inertia_xml->Attribute("ixz"),
              inertia_xml->Attribute("iyy"),
              inertia_xml->Attribute("iyz"),
              inertia_xml->Attribute("izz"));
    return false;
  }

  return true;
}

bool Visual::initXml(TiXmlElement *config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for visual element, using default (Identity)");
    this->origin.clear();
  }
  else if (!this->origin.initXml(o))
  {
    ROS_ERROR("Visual has a malformed origin tag");
    this->origin.clear();
    return false;
  }

  // Geometry
  TiXmlElement *geom = config->FirstChildElement("geometry");
  geometry = parseGeometry(geom);
  if (!geometry)
  {
    ROS_ERROR("Malformed geometry for Visual element");
    return false;
  }

  // Material
  TiXmlElement *mat = config->FirstChildElement("material");
  if (!mat)
  {
    ROS_DEBUG("visual element has no material tag.");
  }
  else
  {
    // get material name
    if (!mat->Attribute("name"))
    {
      ROS_ERROR("Visual material must contain a name attribute");
      return false;
    }
    this->material_name = mat->Attribute("name");

    // try to parse material element in place
    this->material.reset(new Material);
    if (!this->material->initXml(mat))
    {
      ROS_DEBUG("Could not parse material element in Visual block, maybe defined outside.");
      this->material.reset();
    }
    else
    {
      ROS_DEBUG("Parsed material element in Visual block.");
    }
  }

  return true;
}

bool Collision::initXml(TiXmlElement* config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for collision element, using default (Identity)");
    this->origin.clear();
  }
  else if (!this->origin.initXml(o))
  {
    ROS_ERROR("Collision has a malformed origin tag");
    this->origin.clear();
    return false;
  }

  // Geometry
  TiXmlElement *geom = config->FirstChildElement("geometry");
  geometry = parseGeometry(geom);
  if (!geometry)
  {
    ROS_ERROR("Malformed geometry for Collision element");
    return false;
  }

  // Group Tag (optional)
  // collision blocks without a group tag are designated to the "default" group
  const char *group_name_char = config->Attribute("group");
  if (!group_name_char)
    group_name = std::string("default");
  else
    group_name = std::string(group_name_char);

  return true;
}

bool Sphere::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = SPHERE;
  if (!c->Attribute("radius"))
  {
    ROS_ERROR("Sphere shape must have a radius attribute");
    return false;
  }

  try
  {
    radius = boost::lexical_cast<double>(c->Attribute("radius"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
    return false;
  }

  return true;
}

bool Box::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = BOX;
  if (!c->Attribute("size"))
  {
    ROS_ERROR("Box shape has no size attribute");
    return false;
  }
  if (!dim.init(c->Attribute("size")))
  {
    ROS_ERROR("Box shape has malformed size attribute");
    dim.clear();
    return false;
  }
  return true;
}

bool Cylinder::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = CYLINDER;
  if (!c->Attribute("length") ||
      !c->Attribute("radius"))
  {
    ROS_ERROR("Cylinder shape must have both length and radius attributes");
    return false;
  }

  try
  {
    length = boost::lexical_cast<double>(c->Attribute("length"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("length (%s) is not a valid float",c->Attribute("length"));
    return false;
  }

  try
  {
    radius = boost::lexical_cast<double>(c->Attribute("radius"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
    return false;
  }

  return true;
}

bool Mesh::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = MESH;
  if (!c->Attribute("filename"))
  {
    ROS_ERROR("Mesh must contain a filename attribute");
    return false;
  }

  filename = c->Attribute("filename");

  if (c->Attribute("scale"))
  {
    if (!this->scale.init(c->Attribute("scale")))
    {
      ROS_ERROR("Mesh scale was specified, but could not be parsed");
      this->scale.clear();
      return false;
    }
  }
  else
    ROS_DEBUG("Mesh scale was not specified, default to (1,1,1)");

  return true;
}

bool Link::initXml(TiXmlElement* config)
{
  this->clear();

  const char *name_char = config->Attribute("name");
  if (!name_char)
  {
    ROS_ERROR("No name given for the link.");
    return false;
  }
  name = std::string(name_char);

  // Inertial (optional)
  TiXmlElement *i = config->FirstChildElement("inertial");
  if (i)
  {
    inertial.reset(new Inertial);
    if (!inertial->initXml(i))
    {
      ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str());
      return false;
    }
  }

  // Visual (optional)
  TiXmlElement *v = config->FirstChildElement("visual");
  if (v)
  {
    visual.reset(new Visual);
    if (!visual->initXml(v))
    {
      ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str());
      return false;
    }
  }

  // Multiple Collisions (optional)
  for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
  {
    boost::shared_ptr<Collision> col;
    col.reset(new Collision);

    if (col->initXml(col_xml))
    {
      boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols = this->getCollision(col->group_name);
      if (!cols)
      {
        // group does not exist, create one and add to map
        cols.reset(new std::vector<boost::shared_ptr<Collision > >);
        // new group name, create vector, add vector to map and add Collision to the vector
        this->collision_groups.insert(make_pair(col->group_name,cols));
        ROS_DEBUG("successfully added a new collision group name '%s'",col->group_name.c_str());
      }

      // group exists, add Collision to the vector in the map
      cols->push_back(col);
      ROS_DEBUG("successfully added a new collision under group name '%s'",col->group_name.c_str());
    }
    else
    {
      ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str());
      col.reset();
      return false;
    }
  }

  // Collision (optional)
  // Assign one single default collision pointer from the collision_groups map
  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > default_collision = this->getCollision("default");
  if (!default_collision)
  {
    ROS_DEBUG("No 'default' collision group for Link '%s'", this->name.c_str());
  }
  else if (default_collision->empty())
  {
    ROS_DEBUG("'default' collision group is empty for Link '%s'", this->name.c_str());
  }
  else
    this->collision = (*default_collision->begin());

  return true;
}

boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > Link::getCollision(const std::string& group_name) const
{
  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > ptr;
  if (this->collision_groups.find(group_name) == this->collision_groups.end())
    ptr.reset();
  else
    ptr = this->collision_groups.find(group_name)->second;
  return ptr;
}

void Link::setParent(boost::shared_ptr<Link> parent)
{
  this->parent_link_ = parent;
  ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str());
}

void Link::setParentJoint(boost::shared_ptr<Joint> parent)
{
  this->parent_joint = parent;
  ROS_DEBUG("set parent joint '%s' to Link '%s'",  parent->name.c_str(), this->name.c_str());
}

void Link::addChild(boost::shared_ptr<Link> child)
{
  this->child_links.push_back(child);
  ROS_DEBUG("added child Link '%s' to Link '%s'",  child->name.c_str(), this->name.c_str());
}

void Link::addChildJoint(boost::shared_ptr<Joint> child)
{
  this->child_joints.push_back(child);
  ROS_DEBUG("added child Joint '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
}

}

<commit_msg>forgot to reset collision<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Wim Meeussen */

#include "urdf/link.h"
#include <ros/ros.h>
#include <boost/lexical_cast.hpp>

namespace urdf{

boost::shared_ptr<Geometry> parseGeometry(TiXmlElement *g)
{
  boost::shared_ptr<Geometry> geom;
  if (!g) return geom;

  TiXmlElement *shape = g->FirstChildElement();
  if (!shape)
  {
    ROS_ERROR("Geometry tag contains no child element.");
    return geom;
  }

  std::string type_name = shape->ValueStr();
  if (type_name == "sphere")
    geom.reset(new Sphere);
  else if (type_name == "box")
    geom.reset(new Box);
  else if (type_name == "cylinder")
    geom.reset(new Cylinder);
  else if (type_name == "mesh")
    geom.reset(new Mesh);
  else
  {
    ROS_ERROR("Unknown geometry type '%s'", type_name.c_str());
    return geom;
  }

  // clear geom object when fails to initialize
  if (!geom->initXml(shape)){
    ROS_ERROR("Geometry failed to parse");
    geom.reset();
  }

  return geom;
}

bool Material::initXml(TiXmlElement *config)
{
  bool has_rgb = false;
  bool has_filename = false;

  this->clear();

  if (!config->Attribute("name"))
  {
    ROS_ERROR("Material must contain a name attribute");
    return false;
  }

  this->name = config->Attribute("name");

  // texture
  TiXmlElement *t = config->FirstChildElement("texture");
  if (t)
  {
    if (t->Attribute("filename"))
    {
      this->texture_filename = t->Attribute("filename");
      has_filename = true;
    }
    else
    {
      ROS_ERROR("texture has no filename for Material %s",this->name.c_str());
    }
  }

  // color
  TiXmlElement *c = config->FirstChildElement("color");
  if (c)
  {
    if (c->Attribute("rgba"))
    {
      if (!this->color.init(c->Attribute("rgba")))
      {
        ROS_ERROR("Material %s has malformed color rgba values.",this->name.c_str());
        this->color.clear();
        return false;
      }
      else
        has_rgb = true;
    }
    else
    {
      ROS_ERROR("Material %s color has no rgba",this->name.c_str());
    }
  }

  return (has_rgb || has_filename);
}

bool Inertial::initXml(TiXmlElement *config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for inertial element, using default (Identity)");
    this->origin.clear();
  }
  else
  {
    if (!this->origin.initXml(o))
    {
      ROS_ERROR("Inertial has a malformed origin tag");
      this->origin.clear();
      return false;
    }
  }

  TiXmlElement *mass_xml = config->FirstChildElement("mass");
  if (!mass_xml)
  {
    ROS_ERROR("Inertial element must have mass element");
    return false;
  }
  if (!mass_xml->Attribute("value"))
  {
    ROS_ERROR("Inertial: mass element must have value attributes");
    return false;
  }

  try
  {
    mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("mass (%s) is not a float",mass_xml->Attribute("value"));
    return false;
  }

  TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
  if (!inertia_xml)
  {
    ROS_ERROR("Inertial element must have inertia element");
    return false;
  }
  if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
        inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
        inertia_xml->Attribute("izz")))
  {
    ROS_ERROR("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
    return false;
  }
  try
  {
    ixx  = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
    ixy  = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
    ixz  = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
    iyy  = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
    iyz  = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
    izz  = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("one of the inertia elements: ixx (%s) ixy (%s) ixz (%s) iyy (%s) iyz (%s) izz (%s) is not a valid double",
              inertia_xml->Attribute("ixx"),
              inertia_xml->Attribute("ixy"),
              inertia_xml->Attribute("ixz"),
              inertia_xml->Attribute("iyy"),
              inertia_xml->Attribute("iyz"),
              inertia_xml->Attribute("izz"));
    return false;
  }

  return true;
}

bool Visual::initXml(TiXmlElement *config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for visual element, using default (Identity)");
    this->origin.clear();
  }
  else if (!this->origin.initXml(o))
  {
    ROS_ERROR("Visual has a malformed origin tag");
    this->origin.clear();
    return false;
  }

  // Geometry
  TiXmlElement *geom = config->FirstChildElement("geometry");
  geometry = parseGeometry(geom);
  if (!geometry)
  {
    ROS_ERROR("Malformed geometry for Visual element");
    return false;
  }

  // Material
  TiXmlElement *mat = config->FirstChildElement("material");
  if (!mat)
  {
    ROS_DEBUG("visual element has no material tag.");
  }
  else
  {
    // get material name
    if (!mat->Attribute("name"))
    {
      ROS_ERROR("Visual material must contain a name attribute");
      return false;
    }
    this->material_name = mat->Attribute("name");

    // try to parse material element in place
    this->material.reset(new Material);
    if (!this->material->initXml(mat))
    {
      ROS_DEBUG("Could not parse material element in Visual block, maybe defined outside.");
      this->material.reset();
    }
    else
    {
      ROS_DEBUG("Parsed material element in Visual block.");
    }
  }

  return true;
}

bool Collision::initXml(TiXmlElement* config)
{
  this->clear();

  // Origin
  TiXmlElement *o = config->FirstChildElement("origin");
  if (!o)
  {
    ROS_DEBUG("Origin tag not present for collision element, using default (Identity)");
    this->origin.clear();
  }
  else if (!this->origin.initXml(o))
  {
    ROS_ERROR("Collision has a malformed origin tag");
    this->origin.clear();
    return false;
  }

  // Geometry
  TiXmlElement *geom = config->FirstChildElement("geometry");
  geometry = parseGeometry(geom);
  if (!geometry)
  {
    ROS_ERROR("Malformed geometry for Collision element");
    return false;
  }

  // Group Tag (optional)
  // collision blocks without a group tag are designated to the "default" group
  const char *group_name_char = config->Attribute("group");
  if (!group_name_char)
    group_name = std::string("default");
  else
    group_name = std::string(group_name_char);

  return true;
}

bool Sphere::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = SPHERE;
  if (!c->Attribute("radius"))
  {
    ROS_ERROR("Sphere shape must have a radius attribute");
    return false;
  }

  try
  {
    radius = boost::lexical_cast<double>(c->Attribute("radius"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
    return false;
  }

  return true;
}

bool Box::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = BOX;
  if (!c->Attribute("size"))
  {
    ROS_ERROR("Box shape has no size attribute");
    return false;
  }
  if (!dim.init(c->Attribute("size")))
  {
    ROS_ERROR("Box shape has malformed size attribute");
    dim.clear();
    return false;
  }
  return true;
}

bool Cylinder::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = CYLINDER;
  if (!c->Attribute("length") ||
      !c->Attribute("radius"))
  {
    ROS_ERROR("Cylinder shape must have both length and radius attributes");
    return false;
  }

  try
  {
    length = boost::lexical_cast<double>(c->Attribute("length"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("length (%s) is not a valid float",c->Attribute("length"));
    return false;
  }

  try
  {
    radius = boost::lexical_cast<double>(c->Attribute("radius"));
  }
  catch (boost::bad_lexical_cast &e)
  {
    ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
    return false;
  }

  return true;
}

bool Mesh::initXml(TiXmlElement *c)
{
  this->clear();

  this->type = MESH;
  if (!c->Attribute("filename"))
  {
    ROS_ERROR("Mesh must contain a filename attribute");
    return false;
  }

  filename = c->Attribute("filename");

  if (c->Attribute("scale"))
  {
    if (!this->scale.init(c->Attribute("scale")))
    {
      ROS_ERROR("Mesh scale was specified, but could not be parsed");
      this->scale.clear();
      return false;
    }
  }
  else
    ROS_DEBUG("Mesh scale was not specified, default to (1,1,1)");

  return true;
}

bool Link::initXml(TiXmlElement* config)
{
  this->clear();

  const char *name_char = config->Attribute("name");
  if (!name_char)
  {
    ROS_ERROR("No name given for the link.");
    return false;
  }
  name = std::string(name_char);

  // Inertial (optional)
  TiXmlElement *i = config->FirstChildElement("inertial");
  if (i)
  {
    inertial.reset(new Inertial);
    if (!inertial->initXml(i))
    {
      ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str());
      return false;
    }
  }

  // Visual (optional)
  TiXmlElement *v = config->FirstChildElement("visual");
  if (v)
  {
    visual.reset(new Visual);
    if (!visual->initXml(v))
    {
      ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str());
      return false;
    }
  }

  // Multiple Collisions (optional)
  for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
  {
    boost::shared_ptr<Collision> col;
    col.reset(new Collision);

    if (col->initXml(col_xml))
    {
      boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > cols = this->getCollision(col->group_name);
      if (!cols)
      {
        // group does not exist, create one and add to map
        cols.reset(new std::vector<boost::shared_ptr<Collision > >);
        // new group name, create vector, add vector to map and add Collision to the vector
        this->collision_groups.insert(make_pair(col->group_name,cols));
        ROS_DEBUG("successfully added a new collision group name '%s'",col->group_name.c_str());
      }

      // group exists, add Collision to the vector in the map
      cols->push_back(col);
      ROS_DEBUG("successfully added a new collision under group name '%s'",col->group_name.c_str());
    }
    else
    {
      ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str());
      col.reset();
      return false;
    }
  }

  // Collision (optional)
  // Assign one single default collision pointer from the collision_groups map
  this->collision.reset();
  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > default_collision = this->getCollision("default");
  if (!default_collision)
  {
    ROS_DEBUG("No 'default' collision group for Link '%s'", this->name.c_str());
  }
  else if (default_collision->empty())
  {
    ROS_DEBUG("'default' collision group is empty for Link '%s'", this->name.c_str());
  }
  else
    this->collision = (*default_collision->begin());

  return true;
}

boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > Link::getCollision(const std::string& group_name) const
{
  boost::shared_ptr<std::vector<boost::shared_ptr<Collision > > > ptr;
  if (this->collision_groups.find(group_name) == this->collision_groups.end())
    ptr.reset();
  else
    ptr = this->collision_groups.find(group_name)->second;
  return ptr;
}

void Link::setParent(boost::shared_ptr<Link> parent)
{
  this->parent_link_ = parent;
  ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str());
}

void Link::setParentJoint(boost::shared_ptr<Joint> parent)
{
  this->parent_joint = parent;
  ROS_DEBUG("set parent joint '%s' to Link '%s'",  parent->name.c_str(), this->name.c_str());
}

void Link::addChild(boost::shared_ptr<Link> child)
{
  this->child_links.push_back(child);
  ROS_DEBUG("added child Link '%s' to Link '%s'",  child->name.c_str(), this->name.c_str());
}

void Link::addChildJoint(boost::shared_ptr<Joint> child)
{
  this->child_joints.push_back(child);
  ROS_DEBUG("added child Joint '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
}

}

<|endoftext|>


<commit_before>#include <extlib/toml/toml.hpp>
#include <jarngreipr/io/PDBReader.hpp>
#include <jarngreipr/io/PDBWriter.hpp>
#include <jarngreipr/io/write_as_xyz.hpp>
#include <jarngreipr/io/write_as_xyz.hpp>
#include <jarngreipr/model/CGChain.hpp>
#include <jarngreipr/model/make_coarse_grained.hpp>
#include <jarngreipr/forcefield/ForceFieldGenerator.hpp>
#include <jarngreipr/forcefield/ClementiGo.hpp>
#include <mjolnir/math/Vector.hpp>
#include <mjolnir/core/RandomNumberGenerator.hpp>
#include <algorithm>
#include <map>

using mjolnir::operator"" _str;

namespace mjolnir
{

std::vector<char> split_chain_ids(const std::string& key)
{
    if(key.size() == 1)
    {
        if(not std::isupper(key.front()))
        {
            throw std::runtime_error("jarngreipr::split_chain_ids: "
                    "chain ID should be specified in upper case.");
        }
        return std::vector<char>{key.front()};
    }

    if(not (key.size() == 3 && (key.at(1) == '-' || key.at(1) == '&') &&
            std::isupper(key.front()) && std::isupper(key.back())))
    {
        throw std::runtime_error("jarngreipr::split_chain_ids: "
                "chain ID must be upper case and supplied in this way: "
                "'A', 'A-C', or 'A&D'");
    }

    if(key.at(1) == '&')
    {
        // "A&D" -> {A, D}
        return std::vector<char>{key.front(), key.back()};
    }

    // "A-D" -> {A, B, C, D}
    std::vector<char> ids;
    for(char c = key.front(); c <= key.back(); ++c)
    {
        ids.push_back(c);
    }
    return ids;
}

template<typename coordT>
std::vector<std::map<char, PDBChain<coordT>>>
read_reference_structures(const std::vector<toml::Table>& structures)
{
    std::vector<std::map<char, PDBChain<coordT>>> tables;
    for(const auto& conf : structures)
    {
        std::map<char, PDBChain<coordT>> table;
        for(const auto& kvp : conf)
        {
            std::vector<char> chIDs = split_chain_ids(kvp.first);
            const toml::Table val = toml::get<toml::Table>(kvp.second);

            std::string filename;
            try
            {
                filename = toml::get<std::string>(val.at("file"));
            }
            catch(const std::exception& except)
            {
                throw std::runtime_error("jarngreipr::read_reference_structures: "
                        "file is not specified for chain " + kvp.first);
            }

            if(filename.substr(filename.size() - 4) == ".pdb")
            {
                mjolnir::PDBReader<coordT> pdb_reader(filename);
                if(not pdb_reader.is_good())
                {
                    throw std::runtime_error(
                            "jarngreipr::read_reference_structures: "
                            "file open error: filename = " + filename);
                }
                while(not pdb_reader.is_eof())
                {
                    const auto chain = pdb_reader.read_next_chain();
                    const char chain_id = chain.chain_id();
                    const auto found = std::find(
                            chIDs.begin(), chIDs.end(), chain_id);
                    if(found != chIDs.end())
                    {
                        table[*found] = chain;
                        chIDs.erase(found);
                    }
                }
            }
            else
            {
                throw std::runtime_error("jarngreipr::read_reference_structures: "
                        "unrecognizable file: " + filename);
            }

            if(not chIDs.empty())
            {
                std::string mes("jarngreipr::read_reference_structures: "
                                "missing chains in : ");
                mes += filename;
                mes += ", ID = ";
                mes += std::string(chIDs.begin(), chIDs.end());
                throw std::runtime_error(mes);
            }
        }
        tables.push_back(std::move(table));
    }
    return tables;
}

template<typename coordT>
std::vector<std::map<char, PDBChain<coordT>>>
read_initial_structures(const std::vector<toml::Table>& structures)
{
    std::vector<std::map<char, PDBChain<coordT>>> tables;
    for(const auto& conf : structures)
    {
        std::map<char, PDBChain<coordT>> table;
        for(const auto& kvp : conf)
        {
            std::vector<char> chIDs = split_chain_ids(kvp.first);
            const toml::Table val = toml::get<toml::Table>(kvp.second);

            std::string filename;
            try
            {
                filename = toml::get<std::string>(val.at("initial"));
            }
            catch(const std::exception& except)
            {
                // do nothing. initial configuration is optional.
            }
            try
            {
                filename = toml::get<std::string>(val.at("file"));
            }
            catch(const std::exception& except)
            {
                throw std::runtime_error("jarngreipr::read_initial_structures: "
                        "neither file nor initial exists for chain " + kvp.first);
            }

            if(filename.substr(filename.size() - 4) == ".pdb")
            {
                mjolnir::PDBReader<coordT> pdb_reader(filename);
                if(not pdb_reader.is_good())
                {
                    throw std::runtime_error(
                            "jarngreipr::read_reference_structures: "
                            "file open error: " + filename);
                }
                while(not pdb_reader.is_eof())
                {
                    const auto chain = pdb_reader.read_next_chain();
                    const char chain_id = chain.chain_id();
                    const auto found = std::find(
                            chIDs.begin(), chIDs.end(), chain_id);
                    if(found != chIDs.end())
                    {
                        table[*found] = chain;
                        chIDs.erase(found);
                    }
                }
            }
            else
            {
                throw std::runtime_error("jarngreipr::read_reference_structures: "
                        "unrecognizable file: " + filename);
            }

            if(not chIDs.empty())
            {
                std::string mes("jarngreipr::read_reference_structures: "
                                "missing chains in : ");
                mes += filename;
                mes += ", ID = ";
                mes += std::string(chIDs.begin(), chIDs.end());
                throw std::runtime_error(mes);
            }
        }

        tables.push_back(std::move(table));
    }
    return tables;
}

inline std::vector<std::map<char, std::string>>
read_coarse_grained_models(const std::vector<toml::Table>& structures)
{
    std::vector<std::map<char, std::string>> tables;
    for(const auto& conf : structures)
    {
        std::map<char, std::string> table;
        for(const auto& kvp : conf)
        {
            const std::vector<char> chIDs = split_chain_ids(kvp.first);
            const toml::Table val = toml::get<toml::Table>(kvp.second);

            std::string model_name;
            try
            {
                model_name = toml::get<std::string>(val.at("model"));
            }
            catch(const std::exception& except)
            {
                throw std::runtime_error("jarngreipr::read_coarse_grained model: "
                        "model is not specified for chain " + kvp.first);
            }

            for(char id : chIDs)
            {
                table[id] = model_name;
            }
        }
        tables.push_back(std::move(table));
    }
    return tables;
}

template<typename coordT>
std::vector<std::map<char, CGChain<coordT>>>
apply_coarse_grained_models(
        const std::vector<std::map<char, PDBChain<coordT>>>& pdbs,
        const std::vector<std::map<char, std::string>>& models)
{
    assert(pdbs.size() == models.size());
    std::vector<std::map<char, CGChain<coordT>>> cgss;
    for(std::size_t i=0; i<pdbs.size(); ++i)
    {
        std::map<char, CGChain<coordT>> cgs;
        const auto& pdb   = pdbs.at(i);
        const auto& model = models.at(i);

        for(const auto& kv : pdb)
        {
            const auto chid = kv.first;
            const auto&  ch = kv.second;
            cgs[chid] = make_coarse_grained(ch, model.at(chid));
        }

        std::size_t index = 0;
        for(auto& id_chain: cgs)
        {
            for(auto& bead : id_chain.second)
            {
                bead->index() = index;
                ++index;
            }
        }

        cgss.push_back(std::move(cgs));
    }
    return cgss;
}

} // mjolnir

int main(int argc, char **argv)
{
    typedef mjolnir::Vector<double, 3>    coord_type;
    typedef mjolnir::PDBChain<coord_type> pdb_chain_type;
    typedef mjolnir::CGChain<coord_type>  cg_chain_type;

    if(argc != 2)
    {
        std::cerr << "Usage: $ jarngreipr [file.toml]\n";
        std::cerr << "    see input/example.toml for the format" << std::endl;
        return 1;
    }

    const auto input_data = toml::parse(std::string(argv[1]));

    /* prepairing parameters */
    const auto general   = toml::get<toml::Table>(input_data.at("general"));
    const auto file_name = toml::get<std::string>(general.at("file_prefix"));
    std::random_device devrand;
    std::mt19937 mt(devrand());

    /* output general information */{
        std::cout << "[general]\n";
        std::cout << "file_name   = "
                  << toml::get_or(general, "file_prefix", "data"_str) << '\n';
        std::cout << "output_path = "
                  << toml::get_or(general, "output_path", "./"_str) << '\n';
        std::cout << "precision = "
                  << toml::get_or(general, "precision", "double"_str) << '\n';
        std::cout << "boundary  = "
                  << toml::get_or(general, "boundary", "Unlimited"_str) << '\n';
        std::cout << "thread    = " << std::boolalpha
                  << toml::get_or(general, "thread", false) << '\n';
        std::cout << "GPU       = " << std::boolalpha
                  << toml::get_or(general, "GPU",    false) << '\n';
        const std::int64_t default_seed = devrand();
        std::cout << "seed      = "
                  << toml::get_or(general, "seed", default_seed) << '\n';
    }

    const auto parameters =
        toml::parse(toml::get<std::string>(general.at("parameters")));
    const auto phys_paras = toml::get<toml::Table>(parameters.at("physical_constants"));
    const auto vac_permit = toml::get<double>(phys_paras.at("ε0"));

    const auto& mass =
        toml::get<toml::Table>(parameters.at("mass"));
    const auto& phys =
        toml::get<toml::Table>(parameters.at("physical_constants"));

    const auto system_config =
        toml::get<std::vector<toml::Table>>(input_data.at("systems"));

    /* generating coarse-grained structures */
    const auto structure_config =
            toml::get<std::vector<toml::Table>>(input_data.at("structures"));

    // vector<map<char, PDBChain<coord>>>
    // in most cases, the size of vector is one.
    const auto ref_pdbs =
        mjolnir::read_reference_structures<coord_type>(structure_config);
    const auto ini_pdbs =
        mjolnir::read_initial_structures  <coord_type>(structure_config);
    const auto models   =
        mjolnir::read_coarse_grained_models(structure_config);

    // apply coarse-grained model to input pdbs
    const auto ref_cgss = mjolnir::apply_coarse_grained_models(ref_pdbs, models);
    const auto ini_cgss = mjolnir::apply_coarse_grained_models(ini_pdbs, models);

    // output system setting and cg structure
    assert(system_config.size() == ini_cgss.size());
    for(std::size_t i=0; i<system_config.size(); ++i)
    {
        std::cout << "[[systems]]\n";
        // TODO {{{
        const auto& sysconf = system_config.at(i);
        const auto T = toml::get<double>(sysconf.at("temperature"));
        std::cout << "temperature    = " << std::fixed << T << '\n';
        std::cout << "ionic_strength = " << std::fixed
                  << toml::get<double>(sysconf.at("ionic_strength")) << '\n';
        std::cout << "boundary = {";
        for(const auto& kv : toml::get<toml::Table>(sysconf.at("boundary")))
        {
            if(kv.first == "type")
            {
                std::cout << kv.first << "=\"" << toml::get<std::string>(kv.second) << '\"';
            }
            else
            {
                const auto& crd = toml::get<std::array<double, 3>>(kv.second);
                std::cout << kv.first << "=[" << crd[0] << ',' << crd[1] << ','
                          << crd[2] << ']';
            }
        }
        std::cout << "}\n";
        // }}}

        std::cout << "particles = [\n";
        const auto& chain_table = ini_cgss.at(i);
        for(const auto& id_chain_pair : chain_table)
        {
            std::cerr << "chain ID = " << id_chain_pair.first;
            std::cerr << " has " << id_chain_pair.second.size() << " beads." << std::endl;
            for(std::size_t j=0; j<id_chain_pair.second.size(); ++j)
            {
                const auto& bead = id_chain_pair.second.at(j);
                const auto kB = toml::get<double>(phys.at("kB"));
                const auto m  = toml::get<double>(mass.at(bead->name()));
                const auto p  = bead->position();
                std::normal_distribution<double> mxw_blz(0.0, std::sqrt(kB*T/m));
                std::cout << "{mass = " << m
                          <<  ", position = [" << p[0] << ',' << p[1] << ',' << p[2]
                          << "], velocity = [" << mxw_blz(mt) << ','
                          << mxw_blz(mt) << ',' << mxw_blz(mt) << "]},\n";
            }
        }
        std::cout << "]\n";
    }

    const std::unique_ptr<mjolnir::IntraChainForceFieldGenerator<coord_type>>
        ffgen = mjolnir::make_unique<mjolnir::ClementiGo<coord_type>>();

    for(const auto& chains : ref_cgss)
    {
        std::cout << "[[forcefields]]\n";
        for(const auto& idch : chains)
        {
            const auto& ch = idch.second;
            const auto connect = ffgen->generate(std::cout, ch);
        }
    }

    // output parameter

    return 0;
}
<commit_msg>make file_prefix required<commit_after>#include <extlib/toml/toml.hpp>
#include <jarngreipr/io/PDBReader.hpp>
#include <jarngreipr/io/PDBWriter.hpp>
#include <jarngreipr/io/write_as_xyz.hpp>
#include <jarngreipr/io/write_as_xyz.hpp>
#include <jarngreipr/model/CGChain.hpp>
#include <jarngreipr/model/make_coarse_grained.hpp>
#include <jarngreipr/forcefield/ForceFieldGenerator.hpp>
#include <jarngreipr/forcefield/ClementiGo.hpp>
#include <mjolnir/math/Vector.hpp>
#include <mjolnir/core/RandomNumberGenerator.hpp>
#include <algorithm>
#include <map>

using mjolnir::operator"" _str;

namespace mjolnir
{

std::vector<char> split_chain_ids(const std::string& key)
{
    if(key.size() == 1)
    {
        if(not std::isupper(key.front()))
        {
            throw std::runtime_error("jarngreipr::split_chain_ids: "
                    "chain ID should be specified in upper case.");
        }
        return std::vector<char>{key.front()};
    }

    if(not (key.size() == 3 && (key.at(1) == '-' || key.at(1) == '&') &&
            std::isupper(key.front()) && std::isupper(key.back())))
    {
        throw std::runtime_error("jarngreipr::split_chain_ids: "
                "chain ID must be upper case and supplied in this way: "
                "'A', 'A-C', or 'A&D'");
    }

    if(key.at(1) == '&')
    {
        // "A&D" -> {A, D}
        return std::vector<char>{key.front(), key.back()};
    }

    // "A-D" -> {A, B, C, D}
    std::vector<char> ids;
    for(char c = key.front(); c <= key.back(); ++c)
    {
        ids.push_back(c);
    }
    return ids;
}

template<typename coordT>
std::vector<std::map<char, PDBChain<coordT>>>
read_reference_structures(const std::vector<toml::Table>& structures)
{
    std::vector<std::map<char, PDBChain<coordT>>> tables;
    for(const auto& conf : structures)
    {
        std::map<char, PDBChain<coordT>> table;
        for(const auto& kvp : conf)
        {
            std::vector<char> chIDs = split_chain_ids(kvp.first);
            const toml::Table val = toml::get<toml::Table>(kvp.second);

            std::string filename;
            try
            {
                filename = toml::get<std::string>(val.at("file"));
            }
            catch(const std::exception& except)
            {
                throw std::runtime_error("jarngreipr::read_reference_structures: "
                        "file is not specified for chain " + kvp.first);
            }

            if(filename.substr(filename.size() - 4) == ".pdb")
            {
                mjolnir::PDBReader<coordT> pdb_reader(filename);
                if(not pdb_reader.is_good())
                {
                    throw std::runtime_error(
                            "jarngreipr::read_reference_structures: "
                            "file open error: filename = " + filename);
                }
                while(not pdb_reader.is_eof())
                {
                    const auto chain = pdb_reader.read_next_chain();
                    const char chain_id = chain.chain_id();
                    const auto found = std::find(
                            chIDs.begin(), chIDs.end(), chain_id);
                    if(found != chIDs.end())
                    {
                        table[*found] = chain;
                        chIDs.erase(found);
                    }
                }
            }
            else
            {
                throw std::runtime_error("jarngreipr::read_reference_structures: "
                        "unrecognizable file: " + filename);
            }

            if(not chIDs.empty())
            {
                std::string mes("jarngreipr::read_reference_structures: "
                                "missing chains in : ");
                mes += filename;
                mes += ", ID = ";
                mes += std::string(chIDs.begin(), chIDs.end());
                throw std::runtime_error(mes);
            }
        }
        tables.push_back(std::move(table));
    }
    return tables;
}

template<typename coordT>
std::vector<std::map<char, PDBChain<coordT>>>
read_initial_structures(const std::vector<toml::Table>& structures)
{
    std::vector<std::map<char, PDBChain<coordT>>> tables;
    for(const auto& conf : structures)
    {
        std::map<char, PDBChain<coordT>> table;
        for(const auto& kvp : conf)
        {
            std::vector<char> chIDs = split_chain_ids(kvp.first);
            const toml::Table val = toml::get<toml::Table>(kvp.second);

            std::string filename;
            try
            {
                filename = toml::get<std::string>(val.at("initial"));
            }
            catch(const std::exception& except)
            {
                // do nothing. initial configuration is optional.
            }
            try
            {
                filename = toml::get<std::string>(val.at("file"));
            }
            catch(const std::exception& except)
            {
                throw std::runtime_error("jarngreipr::read_initial_structures: "
                        "neither file nor initial exists for chain " + kvp.first);
            }

            if(filename.substr(filename.size() - 4) == ".pdb")
            {
                mjolnir::PDBReader<coordT> pdb_reader(filename);
                if(not pdb_reader.is_good())
                {
                    throw std::runtime_error(
                            "jarngreipr::read_reference_structures: "
                            "file open error: " + filename);
                }
                while(not pdb_reader.is_eof())
                {
                    const auto chain = pdb_reader.read_next_chain();
                    const char chain_id = chain.chain_id();
                    const auto found = std::find(
                            chIDs.begin(), chIDs.end(), chain_id);
                    if(found != chIDs.end())
                    {
                        table[*found] = chain;
                        chIDs.erase(found);
                    }
                }
            }
            else
            {
                throw std::runtime_error("jarngreipr::read_reference_structures: "
                        "unrecognizable file: " + filename);
            }

            if(not chIDs.empty())
            {
                std::string mes("jarngreipr::read_reference_structures: "
                                "missing chains in : ");
                mes += filename;
                mes += ", ID = ";
                mes += std::string(chIDs.begin(), chIDs.end());
                throw std::runtime_error(mes);
            }
        }

        tables.push_back(std::move(table));
    }
    return tables;
}

inline std::vector<std::map<char, std::string>>
read_coarse_grained_models(const std::vector<toml::Table>& structures)
{
    std::vector<std::map<char, std::string>> tables;
    for(const auto& conf : structures)
    {
        std::map<char, std::string> table;
        for(const auto& kvp : conf)
        {
            const std::vector<char> chIDs = split_chain_ids(kvp.first);
            const toml::Table val = toml::get<toml::Table>(kvp.second);

            std::string model_name;
            try
            {
                model_name = toml::get<std::string>(val.at("model"));
            }
            catch(const std::exception& except)
            {
                throw std::runtime_error("jarngreipr::read_coarse_grained model: "
                        "model is not specified for chain " + kvp.first);
            }

            for(char id : chIDs)
            {
                table[id] = model_name;
            }
        }
        tables.push_back(std::move(table));
    }
    return tables;
}

template<typename coordT>
std::vector<std::map<char, CGChain<coordT>>>
apply_coarse_grained_models(
        const std::vector<std::map<char, PDBChain<coordT>>>& pdbs,
        const std::vector<std::map<char, std::string>>& models)
{
    assert(pdbs.size() == models.size());
    std::vector<std::map<char, CGChain<coordT>>> cgss;
    for(std::size_t i=0; i<pdbs.size(); ++i)
    {
        std::map<char, CGChain<coordT>> cgs;
        const auto& pdb   = pdbs.at(i);
        const auto& model = models.at(i);

        for(const auto& kv : pdb)
        {
            const auto chid = kv.first;
            const auto&  ch = kv.second;
            cgs[chid] = make_coarse_grained(ch, model.at(chid));
        }

        std::size_t index = 0;
        for(auto& id_chain: cgs)
        {
            for(auto& bead : id_chain.second)
            {
                bead->index() = index;
                ++index;
            }
        }

        cgss.push_back(std::move(cgs));
    }
    return cgss;
}

} // mjolnir

int main(int argc, char **argv)
{
    typedef mjolnir::Vector<double, 3>    coord_type;
    typedef mjolnir::PDBChain<coord_type> pdb_chain_type;
    typedef mjolnir::CGChain<coord_type>  cg_chain_type;

    if(argc != 2)
    {
        std::cerr << "Usage: $ jarngreipr [file.toml]\n";
        std::cerr << "    see input/example.toml for the format" << std::endl;
        return 1;
    }

    const auto input_data = toml::parse(std::string(argv[1]));

    /* prepairing parameters */
    const auto general   = toml::get<toml::Table>(input_data.at("general"));
    const auto file_name = toml::get<std::string>(general.at("file_prefix"));
    std::random_device devrand;
    std::mt19937 mt(devrand());

    /* output general information */{
        std::cout << "[general]\n";
        std::cout << "file_name   = "
                  << toml::get<std::string>(general.at("file_prefix")) << '\n';
        std::cout << "output_path = "
                  << toml::get_or(general, "output_path", "./"_str) << '\n';
        std::cout << "precision = "
                  << toml::get_or(general, "precision", "double"_str) << '\n';
        std::cout << "boundary  = "
                  << toml::get_or(general, "boundary", "Unlimited"_str) << '\n';
        std::cout << "thread    = " << std::boolalpha
                  << toml::get_or(general, "thread", false) << '\n';
        std::cout << "GPU       = " << std::boolalpha
                  << toml::get_or(general, "GPU",    false) << '\n';
        const std::int64_t default_seed = devrand();
        std::cout << "seed      = "
                  << toml::get_or(general, "seed", default_seed) << '\n';
    }

    const auto parameters =
        toml::parse(toml::get<std::string>(general.at("parameters")));
    const auto phys_paras = toml::get<toml::Table>(parameters.at("physical_constants"));
    const auto vac_permit = toml::get<double>(phys_paras.at("ε0"));

    const auto& mass =
        toml::get<toml::Table>(parameters.at("mass"));
    const auto& phys =
        toml::get<toml::Table>(parameters.at("physical_constants"));

    const auto system_config =
        toml::get<std::vector<toml::Table>>(input_data.at("systems"));

    /* generating coarse-grained structures */
    const auto structure_config =
            toml::get<std::vector<toml::Table>>(input_data.at("structures"));

    // vector<map<char, PDBChain<coord>>>
    // in most cases, the size of vector is one.
    const auto ref_pdbs =
        mjolnir::read_reference_structures<coord_type>(structure_config);
    const auto ini_pdbs =
        mjolnir::read_initial_structures  <coord_type>(structure_config);
    const auto models   =
        mjolnir::read_coarse_grained_models(structure_config);

    // apply coarse-grained model to input pdbs
    const auto ref_cgss = mjolnir::apply_coarse_grained_models(ref_pdbs, models);
    const auto ini_cgss = mjolnir::apply_coarse_grained_models(ini_pdbs, models);

    // output system setting and cg structure
    assert(system_config.size() == ini_cgss.size());
    for(std::size_t i=0; i<system_config.size(); ++i)
    {
        std::cout << "[[systems]]\n";
        // TODO {{{
        const auto& sysconf = system_config.at(i);
        const auto T = toml::get<double>(sysconf.at("temperature"));
        std::cout << "temperature    = " << std::fixed << T << '\n';
        std::cout << "ionic_strength = " << std::fixed
                  << toml::get<double>(sysconf.at("ionic_strength")) << '\n';
        std::cout << "boundary = {";
        for(const auto& kv : toml::get<toml::Table>(sysconf.at("boundary")))
        {
            if(kv.first == "type")
            {
                std::cout << kv.first << "=\"" << toml::get<std::string>(kv.second) << '\"';
            }
            else
            {
                const auto& crd = toml::get<std::array<double, 3>>(kv.second);
                std::cout << kv.first << "=[" << crd[0] << ',' << crd[1] << ','
                          << crd[2] << ']';
            }
        }
        std::cout << "}\n";
        // }}}

        std::cout << "particles = [\n";
        const auto& chain_table = ini_cgss.at(i);
        for(const auto& id_chain_pair : chain_table)
        {
            std::cerr << "chain ID = " << id_chain_pair.first;
            std::cerr << " has " << id_chain_pair.second.size() << " beads." << std::endl;
            for(std::size_t j=0; j<id_chain_pair.second.size(); ++j)
            {
                const auto& bead = id_chain_pair.second.at(j);
                const auto kB = toml::get<double>(phys.at("kB"));
                const auto m  = toml::get<double>(mass.at(bead->name()));
                const auto p  = bead->position();
                std::normal_distribution<double> mxw_blz(0.0, std::sqrt(kB*T/m));
                std::cout << "{mass = " << m
                          <<  ", position = [" << p[0] << ',' << p[1] << ',' << p[2]
                          << "], velocity = [" << mxw_blz(mt) << ','
                          << mxw_blz(mt) << ',' << mxw_blz(mt) << "]},\n";
            }
        }
        std::cout << "]\n";
    }

    const std::unique_ptr<mjolnir::IntraChainForceFieldGenerator<coord_type>>
        ffgen = mjolnir::make_unique<mjolnir::ClementiGo<coord_type>>();

    for(const auto& chains : ref_cgss)
    {
        std::cout << "[[forcefields]]\n";
        for(const auto& idch : chains)
        {
            const auto& ch = idch.second;
            const auto connect = ffgen->generate(std::cout, ch);
        }
    }

    // output parameter

    return 0;
}
<|endoftext|>