#include "aikido/trajectory/util.hpp"

#include <algorithm>
#include <fstream>
#include <iostream>
#include <boost/program_options.hpp>
#include <dart/common/StlHelpers.hpp>
#include "aikido/common/Spline.hpp"
#include "aikido/common/StepSequence.hpp"
#include "aikido/planner/parabolic/ParabolicTimer.hpp"
#include "aikido/statespace/CartesianProduct.hpp"
#include "aikido/statespace/Rn.hpp"
#include "aikido/statespace/SO2.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
#include "aikido/trajectory/Interpolated.hpp"

using aikido::statespace::R;
using aikido::statespace::R1;
using aikido::statespace::SO2;
using aikido::statespace::CartesianProduct;
using aikido::statespace::ConstStateSpacePtr;
using aikido::statespace::GeodesicInterpolator;
using aikido::statespace::StateSpacePtr;
using aikido::statespace::dart::MetaSkeletonStateSpace;
using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
using dart::common::make_unique;

using Eigen::Vector2d;
using LinearSplineProblem
    = aikido::common::SplineProblem<double, int, 2, Eigen::Dynamic, 2>;
using CubicSplineProblem = aikido::common::
    SplineProblem<double, int, 4, Eigen::Dynamic, Eigen::Dynamic>;

namespace aikido {
namespace trajectory {

namespace {

bool checkStateSpace(const statespace::StateSpace* _stateSpace)
{
  // Only supports single-DOF joint spaces, namely R1 and SO2.
  if (dynamic_cast<const R1*>(_stateSpace) != nullptr)
  {
    return true;
  }
  else if (dynamic_cast<const SO2*>(_stateSpace) != nullptr)
  {
    return true;
  }
  else if (auto space = dynamic_cast<const CartesianProduct*>(_stateSpace))
  {
    for (std::size_t isubspace = 0; isubspace < space->getNumSubspaces();
         ++isubspace)
    {
      if (!checkStateSpace(space->getSubspace<>(isubspace).get()))
        return false;
    }
    return true;
  }
  else
  {
    return false;
  }
}

} // namespace

//==============================================================================
UniqueSplinePtr convertToSpline(const Interpolated& inputTrajectory)
{
  using statespace::GeodesicInterpolator;

  if (!std::dynamic_pointer_cast<const GeodesicInterpolator>(
          inputTrajectory.getInterpolator()))
  {
    throw std::invalid_argument(
        "The interpolator of inputTrajectory should be a GeodesicInterpolator");
  }

  const auto stateSpace = inputTrajectory.getStateSpace();
  const auto dimension = stateSpace->getDimension();
  const auto numWaypoints = inputTrajectory.getNumWaypoints();

  if (!checkStateSpace(stateSpace.get()))
    throw std::invalid_argument(
        "convertToSpline only supports Rn and SO2 joint spaces");

  if (numWaypoints == 0)
    throw std::invalid_argument("Trajectory is empty.");

  auto outputTrajectory
      = make_unique<Spline>(stateSpace, inputTrajectory.getStartTime());

  Eigen::VectorXd currentVec, nextVec;
  for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints - 1; ++iwaypoint)
  {
    const auto currentState = inputTrajectory.getWaypoint(iwaypoint);
    double currentTime = inputTrajectory.getWaypointTime(iwaypoint);
    const auto nextState = inputTrajectory.getWaypoint(iwaypoint + 1);
    double nextTime = inputTrajectory.getWaypointTime(iwaypoint + 1);

    stateSpace->logMap(currentState, currentVec);
    stateSpace->logMap(nextState, nextVec);

    // Compute the spline coefficients for this segment of the trajectory.
    LinearSplineProblem problem(
        Vector2d(0, nextTime - currentTime), 2, dimension);
    problem.addConstantConstraint(0, 0, Eigen::VectorXd::Zero(dimension));
    problem.addConstantConstraint(1, 0, nextVec - currentVec);
    const auto spline = problem.fit();

    assert(spline.getCoefficients().size() == 1);
    const auto& coefficients = spline.getCoefficients().front();
    outputTrajectory->addSegment(
        coefficients, nextTime - currentTime, currentState);
  }

  return outputTrajectory;
}

//==============================================================================
UniqueInterpolatedPtr concatenate(
    const Interpolated& traj1, const Interpolated& traj2)
{
  if (traj1.getStateSpace() != traj2.getStateSpace())
    throw std::runtime_error("State space mismatch");

  if (traj1.getInterpolator() != traj2.getInterpolator())
    throw std::runtime_error("Interpolator mismatch");

  auto outputTrajectory = make_unique<Interpolated>(
      traj1.getStateSpace(), traj1.getInterpolator());
  if (traj1.getNumWaypoints() > 1u)
  {
    for (std::size_t i = 0u; i < traj1.getNumWaypoints() - 1u; ++i)
    {
      outputTrajectory->addWaypoint(
          traj1.getWaypointTime(i), traj1.getWaypoint(i));
    }
  }

  const double offset = traj1.getEndTime() - traj2.getStartTime();
  for (std::size_t i = 0u; i < traj2.getNumWaypoints(); ++i)
  {
    outputTrajectory->addWaypoint(
        traj2.getWaypointTime(i) + offset, traj2.getWaypoint(i));
  }

  return outputTrajectory;
}

//==============================================================================
double findTimeOfClosestStateOnTrajectory(
    const Trajectory& traj,
    const Eigen::VectorXd& referenceState,
    double timeStep)
{
  auto stateSpace = traj.getStateSpace();
  const std::size_t configSize
      = static_cast<std::size_t>(referenceState.size());
  if (configSize != stateSpace->getDimension())
    throw std::runtime_error("Dimension mismatch");

  double findTime = traj.getStartTime();
  double minDist = std::numeric_limits<double>::max();

  const common::StepSequence sequence(
      timeStep, true, true, traj.getStartTime(), traj.getEndTime());

  auto currState = stateSpace->createState();
  Eigen::VectorXd currPos(stateSpace->getDimension());
  for (const double currTime : sequence)
  {
    traj.evaluate(currTime, currState);
    stateSpace->logMap(currState, currPos);

    const double currDist = (referenceState - currPos).norm();
    if (currDist < minDist)
    {
      findTime = currTime;
      minDist = currDist;
    }
  }

  return findTime;
}

//==============================================================================
UniqueSplinePtr createPartialTrajectory(
    const Spline& traj, double partialStartTime)
{
  if (partialStartTime < traj.getStartTime()
      || partialStartTime > traj.getEndTime())
  {
    throw std::runtime_error("Wrong partial start time");
  }

  const auto stateSpace = traj.getStateSpace();
  const int dimension = static_cast<int>(stateSpace->getDimension());
  auto outputTrajectory = make_unique<Spline>(stateSpace, traj.getStartTime());

  double currSegmentStartTime = traj.getStartTime();
  double currSegmentEndTime = currSegmentStartTime;
  std::size_t currSegmentIdx = 0u;

  const auto segmentStartState = stateSpace->createState();
  const auto segmentEndState = stateSpace->createState();
  Eigen::VectorXd segStartPos(dimension);
  Eigen::VectorXd segEndPos(dimension);
  Eigen::VectorXd segStartVel(dimension);
  Eigen::VectorXd segEndVel(dimension);
  const Eigen::VectorXd zeroPos = Eigen::VectorXd::Zero(dimension);
  traj.evaluate(partialStartTime, segmentStartState);
  stateSpace->logMap(segmentStartState, segStartPos);
  traj.evaluateDerivative(partialStartTime, 1, segStartVel);

  // Find a segment of original trajectory that includes partialStartTime
  while (currSegmentIdx < traj.getNumSegments())
  {
    currSegmentEndTime += traj.getSegmentDuration(currSegmentIdx);
    if (currSegmentStartTime <= partialStartTime
        && partialStartTime <= currSegmentEndTime)
    {
      // create new segment
      traj.evaluate(currSegmentEndTime, segmentEndState);
      stateSpace->logMap(segmentEndState, segEndPos);
      traj.evaluateDerivative(currSegmentEndTime, 1, segEndVel);

      const double segmentDuration = currSegmentEndTime - partialStartTime;

      if (segmentDuration > 0.0)
      {
        CubicSplineProblem problem(Vector2d{0., segmentDuration}, 4, dimension);
        problem.addConstantConstraint(0, 0, zeroPos);
        problem.addConstantConstraint(0, 1, segStartVel);
        problem.addConstantConstraint(1, 0, segEndPos - segStartPos);
        problem.addConstantConstraint(1, 1, segEndVel);
        const auto solution = problem.fit();
        const auto coefficients = solution.getCoefficients().front();
        outputTrajectory->addSegment(
            coefficients, segmentDuration, segmentStartState);
      }
      break;
    }

    currSegmentIdx++;
    currSegmentStartTime = currSegmentEndTime;
  }

  for (std::size_t i = currSegmentIdx + 1u; i < traj.getNumSegments(); ++i)
  {
    outputTrajectory->addSegment(
        traj.getSegmentCoefficients(i),
        traj.getSegmentDuration(i),
        traj.getSegmentStartState(i));
  }

  return outputTrajectory;
}

//==============================================================================
UniqueInterpolatedPtr toR1JointTrajectory(const Interpolated& trajectory)
{
  if (!checkStateSpace(trajectory.getStateSpace().get()))
    throw std::invalid_argument(
        "toR1JointTrajectory only supports R1 and SO2 joint spaces");

  auto interpolator = std::dynamic_pointer_cast<const GeodesicInterpolator>(
      trajectory.getInterpolator());
  if (!interpolator)
    throw std::invalid_argument(
        "The interpolator of trajectory should be a GeodesicInterpolator");

  // Create new trajectory space.
  auto space = trajectory.getStateSpace();
  std::vector<aikido::statespace::ConstStateSpacePtr> subspaces;
  for (std::size_t i = 0; i < space->getDimension(); ++i)
    subspaces.emplace_back(std::make_shared<const R1>());

  auto rSpace = std::make_shared<CartesianProduct>(subspaces);
  auto rInterpolator = std::make_shared<GeodesicInterpolator>(rSpace);
  auto rTrajectory = make_unique<Interpolated>(rSpace, rInterpolator);

  Eigen::VectorXd sourceVector(space->getDimension());
  auto sourceState = rSpace->createState();

  // Add the first waypoint
  space->logMap(trajectory.getWaypoint(0), sourceVector);
  rSpace->expMap(sourceVector, sourceState);
  rTrajectory->addWaypoint(trajectory.getWaypointTime(0), sourceState);

  auto tangentState = rSpace->createState();
  auto targetState = rSpace->createState();

  // Add the remaining waypoints
  for (std::size_t i = 0; i < trajectory.getNumWaypoints() - 1; ++i)
  {
    const auto tangentVector = interpolator->getTangentVector(
        trajectory.getWaypoint(i), trajectory.getWaypoint(i + 1));

    rSpace->expMap(sourceVector, sourceState);
    rSpace->expMap(tangentVector, tangentState);
    rSpace->compose(sourceState, tangentState, targetState);

    rTrajectory->addWaypoint(trajectory.getWaypointTime(i + 1), targetState);
    rSpace->logMap(targetState, sourceVector);
  }

  return rTrajectory;
}

} // namespace trajectory
} // namespace aikido


/*
   Copyright 2012 efalex <email>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef BOX_SOLVER_HPP
#define BOX_SOLVER_HPP

#include "common.hpp"
#include <boost/numeric/ublas/matrix.hpp>
#include "figure.hpp"
#include "box.hpp"
#include <QThread>

class box_solver: public QThread {
    Q_OBJECT
public:
    const long check_count;

    box_solver();
    void run();
signals:
    void drawBox();
private slots:
    void stopComputing();
    void updateStatus();
private:
    int argc;
    char **argv;
    long count;
    boost::numeric::ublas::vector < long > depth;

    vector_int direction;
    std::vector < figure > figures, solution;
    std::vector < vector_int >solution_pos;
    std::vector < boost::numeric::ublas::matrix < int > >rotate_matrix;
    box b;
    bool stopComputingFlag;
    bool updateStatusFlag;

    void initFigures();
    int solve();
    int iterateFigure(unsigned int);
    void init();
    void initSolution();
    void setRotateVector(boost::numeric::ublas::matrix < int > & , int, int, int, int, int, int);
};

#endif // BOX_SOLVER_HPP


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file orea/engine/sensitivitycube.hpp
    \brief wrapper class for an npvCube and a vector of scenarioDescriotions for use in sensitivity generation
    \ingroup Cube
*/

#pragma once

#include <boost/shared_ptr.hpp>
#include <orea/cube/npvcube.hpp>
#include <orea/scenario/shiftscenariogenerator.hpp>
#include <ql/errors.hpp>
#include <ql/time/date.hpp>
#include <ql/types.hpp>
#include <vector>

namespace ore {
namespace analytics {

class SensitivityCube {

public:
    //! SensitivityCube is a wrapper for an npvCube that gives easier access to the underlying cube elements
    SensitivityCube(const boost::shared_ptr<NPVCube>& cube,
                    boost::shared_ptr<std::vector<ShiftScenarioGenerator::ScenarioDescription>>& scenarioDescriptions,
                    const boost::shared_ptr<ore::data::Portfolio>& portfolio)
        : cube_(cube), scenarioDescriptions_(scenarioDescriptions) {
        for (Size i = 0; i < scenarioDescriptions->size(); i++) {
            ShiftScenarioGenerator::ScenarioDescription desc = (*scenarioDescriptions)[i];

            if (desc.type() == ShiftScenarioGenerator::ScenarioDescription::Type::Up) {
                upFactorIndices_[desc.factor1()] = i;
            } else if (desc.type() == ShiftScenarioGenerator::ScenarioDescription::Type::Down) {
                downFactorIndices_[desc.factor1()] = i;
            } else if (desc.type() == ShiftScenarioGenerator::ScenarioDescription::Type::Cross) {
                std::pair<string, string> pair(desc.factor1(), desc.factor2());
                crossFactorIndices_[pair] = i;
            } else if (desc.type() == ShiftScenarioGenerator::ScenarioDescription::Type::Base) {
                baseFactorIndices_[desc.factor1()] = i;
            }
        }
        for (Size i = 0; i < portfolio->size(); i++) {
            tradeIndices_[portfolio->trades()[i]->id()] = i;
        }
    }

    Real baseNPV(Size tradeIdx) const { return cube_->getT0(tradeIdx, 0); }
    Real baseNPV(string tradeId) const {
        Size tradeIdx = getTradeIndex(tradeId);
        return cube_->getT0(tradeIdx, 0);
    }

    Real upNPV(Size tradeIdx, string factor) const {
        return cubeNPV(tradeIdx, ShiftScenarioGenerator::ScenarioDescription::Type::Up, factor, "");
    }

    Real downNPV(Size tradeIdx, string factor) const {
        return cubeNPV(tradeIdx, ShiftScenarioGenerator::ScenarioDescription::Type::Down, factor, "");
    }

    Real crossNPV(Size tradeIdx, string factor1, string factor2) const {
        return cubeNPV(tradeIdx, ShiftScenarioGenerator::ScenarioDescription::Type::Cross, factor1, factor2);
    }

    Real getNPV(Size i, Size j) const { return cube_->get(i, 0, j, 0); }

    // the scenarioDescriptions_ indices match the cube_ indices for a given type/factor
    Size getFactorIndex(ShiftScenarioGenerator::ScenarioDescription::Type type, string factor1, string factor2) const {
        if (type == ShiftScenarioGenerator::ScenarioDescription::Type::Up) {
            return upFactorIndices_.find(factor1)->second;
        } else if (type == ShiftScenarioGenerator::ScenarioDescription::Type::Down) {
            return downFactorIndices_.find(factor1)->second;
        } else if (type == ShiftScenarioGenerator::ScenarioDescription::Type::Cross) {
            std::pair<string, string> pair(factor1, factor2);
            return crossFactorIndices_.find(pair)->second;
        } else if (type == ShiftScenarioGenerator::ScenarioDescription::Type::Base) {
            return baseFactorIndices_.find(factor1)->second;
        } else {
            QL_FAIL("Invalid type");
        }
    }

    Size getTradeIndex(string tradeId) const {
        auto it = tradeIndices_.find(tradeId);
        return it->second;
    }

    //! Accessors
    const std::map<string, Size>& upFactors() const { return upFactorIndices_; }
    const std::map<string, Size>& downFactors() const { return downFactorIndices_; }
    const std::map<pair<string, string>, Size>& crossFactors() const { return crossFactorIndices_; }

    const boost::shared_ptr<NPVCube>& npvCube() const { return cube_; }
    const boost::shared_ptr<std::vector<ShiftScenarioGenerator::ScenarioDescription>>& scenDesc() const {
        return scenarioDescriptions_;
    }

private:
    Real cubeNPV(Size& tradeIdx, ShiftScenarioGenerator::ScenarioDescription::Type type, string factor1,
                 string factor2 = "") const {
        Size index = getFactorIndex(type, factor1, factor2);
        return cube_->get(tradeIdx, 0, index, 0);
    }

    boost::shared_ptr<NPVCube> cube_;
    boost::shared_ptr<std::vector<ShiftScenarioGenerator::ScenarioDescription>> scenarioDescriptions_;

    std::map<string, Size> tradeIndices_, upFactorIndices_, downFactorIndices_, baseFactorIndices_;
    std::map<pair<string, string>, Size> crossFactorIndices_;
};
} // namespace analytics
} // namespace ore


#include <chrono>
#include <iomanip>
#include <iostream>
#include <memory>

#include <Eigen/Core>

#include "fbcollective/allreduce_ring.h"
#include "fbcollective/allreduce_ring_chunked.h"
#include "fbcollective/barrier_all_to_all.h"
#include "fbcollective/barrier_all_to_one.h"
#include "fbcollective/broadcast_one_to_all.h"
#include "fbcollective/common/logging.h"
#include "fbcollective/context.h"
#include "fbcollective/rendezvous/prefix_store.h"
#include "fbcollective/rendezvous/redis_store.h"
#include "fbcollective/transport/device.h"

#ifdef BENCHMARK_TCP
#include "fbcollective/transport/tcp/device.h"
#endif

#ifdef BENCHMARK_IBVERBS
#include "fbcollective/transport/ibverbs/device.h"
#endif

#include "fbcollective/benchmark/options.h"
#include "fbcollective/benchmark/timer.h"

using namespace fbcollective;
using namespace fbcollective::benchmark;

namespace {

template <typename T>
using EigenVectorMap = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>>;

template <typename T>
using ConstEigenVectorMap =
    Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>>;

static auto kReduce = [](float* x, const float* y, size_t n) {
  EigenVectorMap<float>(x, n) =
      ConstEigenVectorMap<float>(x, n) + ConstEigenVectorMap<float>(y, n);
};

class Benchmark {
 public:
  Benchmark(std::shared_ptr<Context>& context, struct options& options)
      : context_(context), options_(options) {}

  virtual ~Benchmark() {}

  virtual void initialize(int elements) = 0;

  virtual void run() {
    algorithm_->Run();
  }

  virtual bool verify() = 0;

 protected:
  virtual float* allocate(int elements) {
    data_.resize(elements);
    for (int i = 0; i < data_.size(); i++) {
      data_[i] = context_->rank_;
    }
    return data_.data();
  }

  std::shared_ptr<Context> context_;
  struct options options_;
  std::unique_ptr<Algorithm> algorithm_;
  std::vector<float> data_;
};

class AllreduceRingBenchmark : public Benchmark {
  using Benchmark::Benchmark;

 public:
  virtual void initialize(int elements) override {
    auto ptr = allocate(elements);
    algorithm_.reset(
        new AllreduceRing<float>(context_, {ptr}, elements, kReduce));
  }

  virtual bool verify() override {
    auto expected = (context_->size_ * (context_->size_ - 1)) / 2;
    for (int i = 0; i < data_.size(); i++) {
      FBC_ENFORCE_EQ(expected, data_[i], "Mismatch at index ", i);
    }
    return true;
  }
};

class AllreduceRingChunkedBenchmark : public Benchmark {
  using Benchmark::Benchmark;

 public:
  virtual void initialize(int elements) override {
    auto ptr = allocate(elements);
    algorithm_.reset(
        new AllreduceRingChunked<float>(context_, {ptr}, elements, kReduce));
  }

  virtual bool verify() override {
    auto expected = (context_->size_ * (context_->size_ - 1)) / 2;
    for (int i = 0; i < data_.size(); i++) {
      FBC_ENFORCE_EQ(expected, data_[i], "Mismatch at index ", i);
    }
    return true;
  }
};

class BarrierAllToAllBenchmark : public Benchmark {
  using Benchmark::Benchmark;

 public:
  virtual void initialize(int /* unused */) override {
    algorithm_.reset(new BarrierAllToAll(context_));
  }

  virtual bool verify() override {
    return true;
  }
};

class BarrierAllToOneBenchmark : public Benchmark {
  using Benchmark::Benchmark;

 public:
  virtual void initialize(int /* unused */) override {
    // This tool measures at rank=0, so use root=1 for the all to one
    // barrier to measure the end-to-end latency (otherwise we might
    // not account for the send-to-root part of the algorithm).
    algorithm_.reset(new BarrierAllToOne(context_, 1));
  }

  virtual bool verify() override {
    return true;
  }
};

class BroadcastOneToAllBenchmark : public Benchmark {
  using Benchmark::Benchmark;

 public:
  virtual void initialize(int elements) override {
    auto ptr = allocate(elements);
    algorithm_.reset(
        new BroadcastOneToAll<float>(context_, ptr, elements, rootRank_));
  }

  virtual bool verify() override {
    for (int i = 0; i < data_.size(); i++) {
      FBC_ENFORCE_EQ(rootRank_, data_[i], "Mismatch at index ", i);
    }
    return true;
  }

 protected:
  const int rootRank_ = 0;
};

class Runner {
 public:
  using BenchmarkFn =
      std::function<std::unique_ptr<Benchmark>(std::shared_ptr<Context>&)>;

  explicit Runner(const options& options) : options_(options) {
#ifdef BENCHMARK_TCP
    if (options_.transport == "tcp") {
      transport::tcp::attr attr;
      device_ = transport::tcp::CreateDevice(attr);
    }
#endif
#ifdef BENCHMARK_IBVERBS
    if (options_.transport == "ibverbs") {
      transport::ibverbs::attr attr = {
          .name = options_.ibverbsDevice,
          .port = options_.ibverbsPort,
          .index = options_.ibverbsIndex,
      };
      device_ = transport::ibverbs::CreateDevice(attr);
    }
#endif
    FBC_ENFORCE(device_, "Unknown transport: ", options_.transport);

    // Create broadcast algorithm to synchronize between participants
    broadcast_.reset(
        new BroadcastOneToAll<long>(newContext(), &broadcastValue_, 1));
    // Create barrier for run-to-run synchronization
    barrier_.reset(new BarrierAllToOne(newContext()));
  }

  long broadcast(long value) {
    broadcastValue_ = value;
    broadcast_->Run();
    return broadcastValue_;
  }

  std::string newPrefix() {
    std::stringstream prefix;
    prefix << options_.prefix << "-" << prefixCounter_++;
    return prefix.str();
  }

  std::shared_ptr<Context> newContext() {
    auto context =
        std::make_shared<Context>(options_.contextRank, options_.contextSize);
    auto redisStore = std::unique_ptr<rendezvous::Store>(
        new rendezvous::RedisStore(options_.redisHost, options_.redisPort));
    auto prefixStore = std::unique_ptr<rendezvous::Store>(
        new rendezvous::PrefixStore(newPrefix(), redisStore));
    context->connectFullMesh(*prefixStore, device_);
    return context;
  }

  void run(BenchmarkFn& fn) {
    printHeader();

    if (options_.elements > 0) {
      run(fn, options_.elements);
      return;
    }

    // Run sweep over number of elements
    for (int i = 1; i <= 1000000; i *= 10) {
      std::vector<int> js = {i * 1, i * 2, i * 5};
      for (auto& j : js) {
        run(fn, j);
      }
    }
  }

  void run(BenchmarkFn& fn, int n) {
    auto context = newContext();
    auto benchmark = fn(context);
    benchmark->initialize(n);

    // Verify correctness of initial run
    if (options_.verify) {
      benchmark->run();
      FBC_ENFORCE(benchmark->verify());
    }

    // Switch mode based on iteration count or time spent
    auto iterations = options_.iterationCount;
    if (iterations <= 0) {
      FBC_ENFORCE_GT(options_.iterationTimeNanos, 0);

      Distribution warmup;
      for (int i = 0; i < options_.warmupIterationCount; i++) {
        Timer dt;
        benchmark->run();
        warmup.add(dt);
      }

      // Broadcast duration of fastest iteration during warmup,
      // so all nodes agree on the number of iterations to run for.
      auto nanos = broadcast(warmup.min());
      iterations = options_.iterationTimeNanos / nanos;
    }

    // Main benchmark loop
    d_.clear();
    for (int i = 0; i < iterations; i++) {
      Timer dt;
      benchmark->run();
      d_.add(dt);
    }

    printDistribution(n);

    // Barrier to make sure everybody arrived here and the temporary
    // context and benchmark can be destructed.
    barrier_->Run();
  }

  void printHeader() {
    if (options_.contextRank == 0) {
      std::cout << std::setw(11) << "elements" << std::setw(11) << "min (us)"
                << std::setw(11) << "p50 (us)" << std::setw(11) << "p99 (us)"
                << std::setw(11) << "max (us)" << std::setw(11) << "samples"
                << std::endl;
    }
  }

  void printDistribution(int elements) {
    if (options_.contextRank == 0) {
      std::cout << std::setw(11) << elements << std::setw(11)
                << d_.percentile(0.00) / 1000 << std::setw(11)
                << d_.percentile(0.50) / 1000 << std::setw(11)
                << d_.percentile(0.90) / 1000 << std::setw(11)
                << d_.percentile(0.99) / 1000 << std::setw(11) << d_.size()
                << std::endl;
    }
  }

 protected:
  options options_;
  int prefixCounter_ = 0;
  std::shared_ptr<transport::Device> device_;

  long broadcastValue_;
  std::unique_ptr<Algorithm> broadcast_;
  std::unique_ptr<Algorithm> barrier_;

  Distribution d_;
};

} // namespace

// make_unique is a C++14 feature. If we don't have 14, we will emulate
// its behavior. This is copied from folly/Memory.h
#if __cplusplus >= 201402L ||                                              \
    (defined __cpp_lib_make_unique && __cpp_lib_make_unique >= 201304L) || \
    (defined(_MSC_VER) && _MSC_VER >= 1900)
/* using override */ using std::make_unique;
#else

template <typename T, typename... Args>
typename std::enable_if<!std::is_array<T>::value, std::unique_ptr<T>>::type
make_unique(Args&&... args) {
  return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
}

// Allows 'make_unique<T[]>(10)'. (N3690 s20.9.1.4 p3-4)
template <typename T>
typename std::enable_if<std::is_array<T>::value, std::unique_ptr<T>>::type
make_unique(const size_t n) {
  return std::unique_ptr<T>(new typename std::remove_extent<T>::type[n]());
}

// Disallows 'make_unique<T[10]>()'. (N3690 s20.9.1.4 p5)
template <typename T, typename... Args>
typename std::enable_if<std::extent<T>::value != 0, std::unique_ptr<T>>::type
make_unique(Args&&...) = delete;

#endif

int main(int argc, char** argv) {
  auto x = benchmark::parseOptions(argc, argv);

  Runner::BenchmarkFn fn;
  if (x.benchmark == "allreduce_ring") {
    fn = [&](std::shared_ptr<Context>& context) {
      return make_unique<AllreduceRingBenchmark>(context, x);
    };
  } else if (x.benchmark == "allreduce_ring_chunked") {
    fn = [&](std::shared_ptr<Context>& context) {
      return make_unique<AllreduceRingChunkedBenchmark>(context, x);
    };
  } else if (x.benchmark == "barrier_all_to_all") {
    fn = [&](std::shared_ptr<Context>& context) {
      return make_unique<BarrierAllToAllBenchmark>(context, x);
    };
  } else if (x.benchmark == "barrier_all_to_one") {
    fn = [&](std::shared_ptr<Context>& context) {
      return make_unique<BarrierAllToOneBenchmark>(context, x);
    };
  } else if (x.benchmark == "broadcast_one_to_all") {
    fn = [&](std::shared_ptr<Context>& context) {
      return make_unique<BroadcastOneToAllBenchmark>(context, x);
    };
  }

  if (!fn) {
    FBC_ENFORCE(false, "Invalid algorithm: ", x.benchmark);
  }

  Runner r(x);
  r.run(fn);
  return 0;
}


// -------------------------------------------
//  @description: 根据彩色图像的像素产生点云代码 产生三维骨架点云 计算外积 计算方差 方差保存供svm训练
//  @author: hts
//  @data: 2020-04-20
//  @version: wpdwp
// -------------------------------------------
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <cmath>
#include <mutex>
#include <thread>
#include <chrono>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>

#include <opencv2/opencv.hpp>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <kinect2_bridge/kinect2_definitions.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
#define pi 3.14159265359

class Posepoint
{
private:
  std::mutex lock;
  cv::Mat color, depth;
  cv::Mat cameraMatrixColor, cameraMatrixDepth;
  cv::Mat lookupX, lookupY;
  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud;
  pcl::PCDWriter writer;
  bool visualize_stop;
  double pick_points[24];
  // double pick_points2[18];
  Vector3f Lshoulder, Rshoulder, Pelv;
  Vector3f Head,Neck,Thrx,Lhip,Lknee,Lankle,Rhip,Rknee,Rankle;
  std::string   pic_class, person_class, pic_name, depth_name, color_path, depth_path;
  int sp;
  int op;
  bool right;
public:
  Posepoint():visualize_stop(false),sp(0),op(0)
  {
    cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F);
    // cameraMatrixDepth = cv::Mat::zeros(3, 3, CV_64F);
    // static double my_points[6] = {255.67636108398438,292.16888427734375,320.4430236816406,266.26220703125,330.15802001953125,298.64556884765625};
    // static double my_points2[18] = {191.4891815185547, 215.0364227294922, 218.52777099609375, 226.16993713378906, 248.7473602294922, 234.1224822998047, 263.0619201660156, 219.8079376220703, 231.25181579589844, 221.3984375, 228.07080078125, 207.08389282226562, 259.88092041015625, 227.7604522705078, 296.4625244140625, 262.7515869140625, 301.23406982421875, 264.34210205078125, 315.548583984375, 283.42816162109375};
    for (int i=1; i<24; i++)
    {
      // fscanf(fq,"%f\n",&pick_points[i]);//%lf之间应该有逗号，因为没有逗号只能读第一个数。用&是因为要把数存到对应数组元素的地址中去。\n是换行读取
      // cout<<pick_points[i]<<endl;
      pick_points[i] = 1;
    }
  }
  ~Posepoint()
  {
  }
public:
    void start()
    {
      FILE  *fq;
      fq=fopen("/home/hts/Desktop/kinect_keypoints/a/test_keypoints.txt" ,"rt+");//"rt+"是打开一个文本文件，可以读写。
      while(!feof(fq))
      {
        if(fscanf(fq,"%lf\n",&pick_points[0])==1)
        {
          // cout<<pick_points[0]<<endl;
            for (int i=1; i<24; i++)
          {
            fscanf(fq,"%lf\n",&pick_points[i]);//%lf之间应该有逗号，因为没有逗号只能读第一个数。用&是因为要把数存到对应数组元素的地址中去。\n是换行读取
            // cout<<pick_points[i]<<endl;
          }
          char * a,*b,*c,*d;
          a= new char [3];
          b= new char [1];
          c= new char [14];
          d = new char [14];
          fscanf(fq,"%s", a);
          fscanf(fq,"%s", b);
          fscanf(fq,"%s", c);
          pic_class = a;
          person_class = b;
          pic_name = c;
          for(int i=0; i<5;i++)
          {
            d[i] =c[i];
          }
          d[5]='d';d[6]='e';d[7]='p';d[8]='t';d[9]='h';d[10]='.';d[11]='p';d[12]='n';d[13]='g';
          depth_name = d;
          // depth_name = depth_name+"depth.png";
          // depth_name = depth_name.substring(0, depth_name.length() -1);
          color_path = "/home/hts/Desktop/kinect_pic/"+person_class+'/'+ pic_class+'/'+pic_name;
          depth_path = "/home/hts/Desktop/kinect_depth/"+person_class+'/'+pic_class+'/'+depth_name;
          // cout<<color_path<<endl;
          // cout<<depth_path<<endl;
          begin(color_path,depth_path);
          // ThreeCross();
          Transformation();
          calculate();
          // if(!right)
          // {
          //   cout<<color_path<<endl;
          // }
        }
      }
      // cout<<"爬识别正确："<<sp<<"  爬识别错误："<<op<<endl;
    }
    void begin(std::string color_path, std::string depth_path)
  {
    // cout<<"开始!"<<endl;
    color = cv::imread(color_path);
    depth = cv::imread(depth_path,2);
    // cv::imshow("color", color);
    // cout<<"!!!"<<endl;
    // cv::imshow("depth", depth);
    // cv::waitKey(0);
    cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>());
    cloud->height = color.rows;
    cloud->width = color.cols;
    cloud->is_dense = false;
    cloud->points.resize(cloud->height * cloud->width);
    readCameraInfo();
    createLookup(this->color.cols, this->color.rows); //像极坐标系中的值保存在lookupX　lookupY里面
    cloudViewer();
  }
  // 3x3内参矩阵
//   367.933   0         254.169
//     0     367.933     204.267
//     0       0            1
    void readCameraInfo()
  {
    double cameraInfoK[9]={367.933 , 0 , 254.169,
                           0 , 367.933 , 204.267,
                          0 , 0 , 1             };
    double *itC = cameraMatrixColor.ptr<double>(0, 0);
    for(size_t i = 0; i < 9; ++i, ++itC)
    {
      *itC = cameraInfoK[i];
      // cout<<*itC<<endl;
    }
    // cout<<cameraMatrixColor.at<double>(0, 0)<<endl;
  }
    // 求在相机坐标系中的坐标
    void createLookup(size_t width, size_t height)
  {
    // width是列->x　height是行->y
    // 得到相机的内参数
    //  成像模型
// [u       [ fx　0 cx   [x
//  v  = 1/z  0  fy cy    y
//  1]        0   0  1]   z]
// 求逆矩阵　得到像极坐标系中的坐标
    const float fx = 1.0f / cameraMatrixColor.at<double>(0, 0);
    const float fy = 1.0f / cameraMatrixColor.at<double>(1, 1);
    const float cx = cameraMatrixColor.at<double>(0, 2);
    const float cy = cameraMatrixColor.at<double>(1, 2);
    float *it;
    // cout<<"相机的内参:"<<fx<<" "<<fy<<" "<<cx<<" "<<cy<<" "<<endl;
    lookupY = cv::Mat(1, height, CV_32F);
    it = lookupY.ptr<float>();
    for(size_t r = 0; r < height; ++r, ++it)
    {
      *it = (r - cy) * fy;
      // cout<<*it<<endl;
    }

    lookupX = cv::Mat(1, width, CV_32F);
    it = lookupX.ptr<float>();
    for(size_t c = 0; c < width; ++c, ++it)
    {
      *it = (c - cx) * fx;
      // cout<<*it<<endl;
    }
  }
    void cloudViewer()
  {
    cv::Mat color, depth;
    // pcl::visualization::PCLVisualizer::Ptr visualizer(new pcl::visualization::PCLVisualizer("Cloud Viewer"));
    const std::string cloudName = "rendered";

    lock.lock();
    color = this->color;
    depth = this->depth;
    // updateCloud = false;
    lock.unlock();

    createCloud(depth, color, cloud);
    // 将点云数据添加到视窗中，并为其定义一个唯一的字符串作为ID号，利用此ID号保证其他成员方法也能表示该点云。
    // 多次调用addPointCloud()可以实现多个点云的叠加，每调用一次就创建一个新的ID号。如果想要更新一个已经
    // 显示的点云，用户必须先调用removePointCloud()，并提供新的ID号。（在PCL1.1版本之后直接调用updatePointCloud()
    //  就可以了，不必手动调用removePointCloud()就可实现点云更新）

    // visualizer->addPointCloud(cloud, cloudName);
    // // 修改现实点云的尺寸。用户可通过该方法控制点云在视窗中的显示方式
    // visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloudName);
    // // 设置XYZ三个坐标轴的大小和长度，该值也可以缺省
    // // 查看复杂的点云图像会让用户没有方向感，为了让用户保持正确的方向判断，需要显示坐标轴。三个坐标轴X（R，红色）
    // // Y（G，绿色）Z（B，蓝色）分别用三种不同颜色的圆柱体代替
    // visualizer->addCoordinateSystem(1.0);
    // // 通过设置相机参数是用户从默认的角度和方向观察点
    // visualizer->initCameraParameters();
    // // 设置窗口viewer的背景颜色
    // visualizer->setBackgroundColor(0, 0, 0);
    // // visualizer->setPosition(mode == BOTH ? color.cols : 0, 0);
    // visualizer->setPosition(0, 0);
    // visualizer->setSize(color.cols, color.rows);
    // visualizer->setShowFPS(true);
    // visualizer->setCameraPosition(0, 0, 0, 0, -1, 0);
    // visualizer->registerKeyboardCallback(&Posepoint::keyboardEvent, *this);
    // saveCloud(cloud);
    // // visualizer->spinOnce(300);
    // while (!visualize_stop) {
    //     visualizer->spinOnce(100);
    //     // boost::this_thread::sleep(boost::posix_time::microseconds(1000));
    // }

    // visualizer->close();

  }
    void createCloud(const cv::Mat &depth, const cv::Mat &color, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
  {
    int body_part;
    const float badPoint = std::numeric_limits<float>::quiet_NaN();
    // #pragma omp parallel for
    for(int r = 0; r < depth.rows; ++r)
    {
      // 创建点云row行，每一行有col列
      pcl::PointXYZRGBA *itP = &cloud->points[r * depth.cols];
      // ptr函数访问任意一行像素的首地址
      const uint16_t *itD = depth.ptr<uint16_t>(r);
      const cv::Vec3b *itC = color.ptr<cv::Vec3b>(r);
      const float y = lookupY.at<float>(0, r);
      const float *itX = lookupX.ptr<float>();

        for(size_t c = 0; c < (size_t)depth.cols; ++c, ++itP, ++itD, ++itC, ++itX)
      {
        register const float depthValue = *itD / 1000.0f;
        // Check for invalid measurements
        if(*itD == 0)
        {
          // not valid
          itP->x = itP->y = itP->z = badPoint;
          itP->rgba = 0;
          continue;
        }
        itP->z = depthValue;
        itP->x = *itX * depthValue;
        itP->y = y * depthValue;
        itP->b = itC->val[0];
        itP->g = itC->val[1];
        itP->r = itC->val[2];
        itP->a = 255;
          bool picked = false;
          if(size_t(pick_points[0]) == c && int(pick_points[1]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 0;
        }
          if(size_t(pick_points[2]) == c && int(pick_points[3]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 1;
        }
          if(size_t(pick_points[4]) == c && int(pick_points[5]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 2;
        }
          if(size_t(pick_points[6]) == c && int(pick_points[7]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 3;
        }
          if(size_t(pick_points[8]) == c && int(pick_points[9]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 4;
        }
          if(size_t(pick_points[10]) == c && int(pick_points[11]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 5;
        }
          if(size_t(pick_points[12]) == c && int(pick_points[13]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 6;
        }
          if(size_t(pick_points[14]) == c && int(pick_points[15]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 7;
        }
          if(size_t(pick_points[16]) == c && int(pick_points[17]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 8;
        }
          if(size_t(pick_points[18]) == c && int(pick_points[19]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 9;
        }
          if(size_t(pick_points[20]) == c && int(pick_points[21]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 12;
        }
          if(size_t(pick_points[22]) == c && int(pick_points[23]) == r)
        {
            // cout<<"点("<<r<<","<<c<<")选中"<<endl;
            picked = true;
            body_part = 13;
        }
        if (picked)
        {
          register const float depthValue = *itD / 1000.0f;
          switch(body_part)
          {
          case 0:;
                Rankle(0) = itP->x; Rankle(1) = itP->y; Rankle(2) = itP->z;
                break;
          case 1:
                Rknee(0) = itP->x; Rknee(1) = itP->y; Rknee(2) = itP->z;
                break;
          case 2:
                Rhip(0) = itP->x; Rhip(1) = itP->y; Rhip(2) = itP->z;
                break;
          case 3:
                Lhip(0) = itP->x; Lhip(1) = itP->y; Lhip(2) = itP->z;
                break;
          case 4:
                Lknee(0) = itP->x; Lknee(1) = itP->y; Lknee(2) = itP->z;
                break;
          case 5:
                Lankle(0) = itP->x; Lankle(1) = itP->y; Lankle(2) = itP->z;
                break;
          case 6:
                Pelv(0) = itP->x; Pelv(1) = itP->y; Pelv(2) = itP->z;
                break;
          case 7:
                Thrx(0) = itP->x; Thrx(1) = itP->y; Thrx(2) = itP->z;
                break;
          case 8:
                Neck(0) = itP->x; Neck(1) = itP->y; Neck(2) = itP->z;
                break;
          case 9:
                Head(0) = itP->x; Head(1) = itP->y; Head(2) = itP->z;
                break;
          case 12:
                Rshoulder(0) = itP->x; Rshoulder(1) = itP->y; Rshoulder(2) = itP->z;
                break;
          case 13:
                Lshoulder(0) = itP->x; Lshoulder(1) = itP->y; Lshoulder(2) = itP->z;
                break;
          }
        }
      }
    }
  }
    void saveCloud(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud)
  {
    std::ostringstream oss;
    oss.str("");
    oss << "/home/hts/Desktop/kinect_cloud/a/aaa/" << std::setfill('0') << std::setw(4);
    const std::string baseName = oss.str();
    const std::string cloudName = baseName + "_cloud.pcd";
    OUT_INFO("saving cloud: " << cloudName);
     // writer是该类的pcl::PCDWriter类型的成员变量
    writer.writeBinary(cloudName, *cloud);
  }
    void keyboardEvent(const pcl::visualization::KeyboardEvent &event, void *)
  {
    if(event.keyUp())
    {
      switch(event.getKeyCode())
      {
      case 27:
      case 'q':
        visualize_stop = true;
        break;
      }
    }
  }
    //叉积
  Vector3f ThreeCross()
  {
    Vector3f a, b,result;
    a=Lshoulder-Pelv;
    b=Rshoulder-Pelv;
    // cout<<Lshoulder<<endl;cout<<Rshoulder<<endl;cout<<Pelv<<endl;
    // cout<<"左肩向量:\n"<<a<<"   右肩向量:\n"<<b<<endl;
    result = a.cross(b);
    // cout<<"躯干法向量:\n"<<result<<endl;
    // Eigen::Vector3d v3(0, 0, 0);
	  // v3.x() = 1;
	  // v3[2] = 1;
	  AngleAxisd angle_axis3(pi *25/ 18, Eigen::Vector3d(1, 0, 0));//1系绕x轴逆时针旋转250得到2系
    // angle_axis3.matrix().cast<float>()
	  Vector3f rotated_result = angle_axis3.matrix().cast<float>()*result;
	  // cout << "绕x轴顺时针旋转250°(Rcw):" << endl << angle_axis3.matrix() << endl;
	  // cout << "躯干法向量旋转后:" << endl << rotated_result.transpose() << endl;
    if (rotated_result(2)<0)
    {
      sp+=1;
      right = true;
    }
    if (rotated_result(2)>0)
    {
      op+=1;
      right = false;
    }
    return rotated_result;
  }
  //转换到世界坐标系
  void  Transformation()
  {
    // Vector3f result = ori;
    // // 旋转矩阵就是3x3的矩阵
    // Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量(方向与旋转轴[1,0,0]相同,模为旋转的角度250°)
    Eigen::AngleAxisd rotation_vector (pi *25/ 18,Eigen::Vector3d(1,0,0));
    cout.precision(3);//指定输出的精度
    cout<<"Rwc旋转矩阵为：\n"<<rotation_vector.matrix()<<endl;
    // 三维的齐次变换矩阵是4x4的矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    // 根据旋转向量进行旋转（注意旋转向量包含了旋转轴和旋转了的角度）
    T.rotate(rotation_vector);
    cout<<"旋转后的变换矩阵为：\n"<<T.matrix()<<endl;
    T.pretranslate(Eigen::Vector3d(0,0,1.35)); // 把第四列的平移向量设置为(1,3,4)
    cout<<"设置平移量后的矩阵为：\n"<<T.matrix()<<endl;
    // result = T*ori;
    Head = T.cast<float>()*Head; Neck = T.cast<float>()*Neck; Thrx = T.cast<float>()*Thrx; Pelv = T.cast<float>()*Pelv; Lhip = T.cast<float>()*Lhip; Lknee = T.cast<float>()*Lknee; Lankle = T.cast<float>()*Lankle; Rhip = T.cast<float>()*Rhip; Rknee = T.cast<float>()*Rknee; Rankle = T.cast<float>()*Rankle; Lshoulder = T.cast<float>()*Lshoulder; Rshoulder = T.cast<float>()*Rshoulder;
    cout<<Head<<endl;
    cout<<Neck<<endl;
    cout<<Thrx<<endl;
    cout<<Pelv<<endl;
    cout<<Lhip<<endl;
    cout<<Lknee<<endl;
    cout<<Lankle<<endl;
    cout<<Rhip<<endl;
    cout<<Rknee<<endl;
    cout<<Rankle<<endl;
  }
  double calculate()
  {
    double sum =0;
    sum = Head(2) + Neck(2) + Thrx(2) + Pelv(2) + Lhip(2) + Rhip(2)+Rshoulder(2)+Lshoulder(2);
    cout<<"和"<<sum<<endl;
    double mean = sum/10;
    cout<<"均值:"<<mean<<endl;
    double stdvar = sqrt(((Head(2)-mean)*(Head(2)-mean)+(Neck(2)-mean)*(Neck(2)-mean)+(Thrx(2)-mean)*(Thrx(2)-mean)+(Pelv(2)-mean)*(Pelv(2)-mean)+(Lhip(2)-mean)*(Lhip(2)-mean)+(Rhip(2)-mean)*(Rhip(2)-mean)+(Rshoulder(2)-mean)*(Rshoulder(2)-mean)+(Lshoulder(2)-mean)*(Lshoulder(2)-mean))/10);
    cout<<"标准差为:"<<stdvar<<endl;
    // if(stdvar<0.2)
    // {
    //   right = true;
    //   sp+=1;
    // }
    // else
    // {
    //   right =false;
    //   op+=1;
    // }
    FILE *fp = NULL;
    fp = fopen("/home/hts/catkin_ws/var.txt", "a+");
    fprintf(fp, "%lf\n%lf\n", mean,stdvar);
    fclose(fp);
    return stdvar;
  }

};

int main(int argc, char**argv)
{
  // std::string color_path = argv[1];
  // std::string depth_path = argv[2];
  Posepoint posepoint;
  posepoint.start();
  // cout<<"!!";
  // posepoint.ThreeCross();
  // posepoint.Transformation();
  // posepoint.calculate();
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_WHIncoherentAdjointPhotonScatteringDistribution_def.hpp
//! \author Alex Robinson
//! \brief  The Waller-Hartree adjoint incoherent photon scattering dist. def.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_WH_INCOHERENT_ADJOINT_PHOTON_SCATTERING_DISTRIBUTION_DEF_HPP
#define MONTE_CARLO_WH_INCOHERENT_ADJOINT_PHOTON_SCATTERING_DISTRIBUTION_DEF_HPP

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "Utility_InverseLengthConversionPolicy.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_ContractException.hpp"

namespace MonteCarlo{

// Constructor
template<typename ScatteringFunctionArgUnitConversionPolicy>
WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::WHIncoherentAdjointPhotonScatteringDistribution(
     const double max_energy,
     const Teuchos::RCP<const Utility::OneDDistribution>& scattering_function )
  : IncoherentAdjointPhotonScatteringDistribution( max_energy ),
    d_scattering_function( scattering_function )
{
  // Make sure the unit conversion policy is valid
  testStaticPrecondition( (boost::is_same<typename ScatteringFunctionArgUnitConversionPolicy::Dimension,Utility::InverseLengthDimension>::value) );
  // Make sure the scattering function is valid
  testPrecondition( !scattering_function.is_null() );
}

// Evaluate the distribution
template<typename ScatteringFunctionArgUnitConversionPolicy>
double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluate(
				   const double incoming_energy,
				   const double max_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= max_energy );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >=
		    calculateMinScatteringAngleCosine( incoming_energy,
						       max_energy ));
  testPrecondition( scattering_angle_cosine <= 1.0 );

  const double scattering_function_value =
    this->evaluateScatteringFunction( incoming_energy,
				      max_energy,
				      scattering_angle_cosine );

  const double diff_kn_cross_section =
    this->evaluateAdjointKleinNishinaDist( incoming_energy,
					   max_energy,
					   scattering_angle_cosine );

  return diff_kn_cross_section*scattering_function_value;
}

// Evaluate the integrated cross section (b)
template<typename ScatteringFunctionArgUnitConversionPolicy>
double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluateIntegratedCrossSection(
						 const double incoming_energy,
						 const double max_energy,
						 const double precision ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= max_energy );

  // Evaluate the integrated cross section
  boost::function<double (double x)> diff_cs_wrapper =
    boost::bind<double>( &WHIncoherentAdjointPhotonScatteringDistribution::evaluate,
			 boost::cref( *this ),
			 incoming_energy,
			 max_energy,
			 _1 );

  double abs_error, integrated_cs;

  Utility::GaussKronrodIntegrator quadrature_gkq_int( precision );

  const double min_scattering_angle_cosine =
    calculateMinScatteringAngleCosine( incoming_energy, max_energy );

  quadrature_gkq_int.integrateAdaptively<15>( diff_cs_wrapper,
					     min_scattering_angle_cosine,
					     1.0,
					     integrated_cs,
					     abs_error );

  // Make sure the integrated cross section is valid
  testPostcondition( integrated_cs >= 0.0 );

  return integrated_cs;
}

// Sample an outgoing energy and direction from the distribution
/*! \details This function will only sample an adjoint Compton line energy (no
 * Doppler broadening).
 */
template<typename ScatteringFunctionArgUnitConversionPolicy>
void WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::sample(
					const double incoming_energy,
					double& outgoing_energy,
					double& scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= this->getMaxEnergy() );

  unsigned trial_dummy;

  return this->sampleAndRecordTrials( incoming_energy,
				      outgoing_energy,
				      scattering_angle_cosine,
				      trial_dummy );
}

// Sample an outgoing energy and direction and record the number of trials
/*! \details This function will only sample an adjoint Compton line energy (no
 * Doppler broadening).
 */
template<typename ScatteringFunctionArgUnitConversionPolicy>
void WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::sampleAndRecordTrials(
					       const double incoming_energy,
					       double& outgoing_energy,
					       double& scattering_angle_cosine,
					       unsigned& trials ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= this->getMaxEnergy() );

  // Evaluate the maximum scattering function value
  const double min_scattering_angle_cosine =
    calculateMinScatteringAngleCosine( incoming_energy, this->getMaxEnergy() );

  const double max_scattering_function_value =
    this->evaluateScatteringFunction( incoming_energy,
				      min_scattering_angle_cosine );

  while( true )
  {
    this->sampleAndRecordTrialsAdjointKleinNishina( incoming_energy,
						    outgoing_energy,
						    scattering_angle_cosine,
						    trials );

    const double scattering_function_value =
      this->evaluateScatteringFunction( incoming_energy,
					scattering_angle_cosine );

    const double scaled_random_number = max_scattering_function_value*
      Utility::RandomNumberGenerator::getRandomNumber<double>();

    if( scaled_random_number <= scattering_function_value )
      break;
  }

  // Make sure the scattering angle cosine is valid
  testPostcondition( scattering_angle_cosine >= min_scattering_angle_cosine );
  testPostcondition( scattering_angle_cosine <= 1.0 );
  // Make sure the adjoint Compton line energy is valid
  testPostcondition( outgoing_energy >= incoming_energy );
}

// Randomly scatter the photon and return the shell that was interacted with
template<typename ScatteringFunctionArgUnitConversionPolicy>
void WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::scatterAdjointPhoton(
				     AdjointPhotonState& adjoint_photon,
				     ParticleBank& bank,
				     SubshellType& shell_of_interaction ) const
{
  // Make sure the adjoint photon energy is valid
  testPrecondition( adjoint_photon.getEnergy() <= this->getMaxEnergy() );

  // Generate probe particles
  this->createProbeParticles( adjoint_photon, bank );

  // Scattering the adjoint photon
  double outgoing_energy, scattering_angle_cosine;

  this->sample( adjoint_photon.getEnergy(),
		outgoing_energy,
		scattering_angle_cosine );

  shell_of_interaction = UNKNOWN_SUBSHELL;

  adjoint_photon.setEnergy( outgoing_energy );

  adjoint_photon.rotateDirection( scattering_angle_cosine,
				  this->sampleAzimuthalAngle() );
}

// Check if an energy is above the scattering window
/*! \details Becuase of the scattering function evaluating to zero when
 * the scattering angle cosine is 1.0, the scattering window upper bound must
 * exclude the energy of interest.
 */
template<typename ScatteringFunctionArgUnitConversionPolicy>
bool WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::isEnergyAboveScatteringWindow(
				            const double energy_of_interest,
				            const double initial_energy ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( initial_energy > 0.0 );
  // Make sure the energy of interest is valid
  testPrecondition( energy_of_interest >= 0.0 );

  return initial_energy >= energy_of_interest;
}

// Evaluate the scattering function
template<typename ScatteringFunctionArgUnitConversionPolicy>
inline double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluateScatteringFunction(
				   const double incoming_energy,
				   const double max_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= max_energy );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >=
		    calculateMinScatteringAngleCosine( incoming_energy,
						       max_energy ));
  testPrecondition( scattering_angle_cosine <= 1.0 );

  // Calculate the outgoing energy
  const double outgoing_energy =
    calculateAdjointComptonLineEnergy( incoming_energy,
				       scattering_angle_cosine );

  // Calculate the inverse wavelength of the outgoing photon (1/cm)
  const double inverse_wavelength = outgoing_energy/
    (Utility::PhysicalConstants::planck_constant*
     Utility::PhysicalConstants::speed_of_light);

  // The scattering function argument (1/cm)
  double scattering_function_arg =
    sqrt( (1.0 - scattering_angle_cosine)/2.0 )*inverse_wavelength;

  ScatteringFunctionArgUnitConversionPolicy::convertFromNativeUnits(
						     scattering_function_arg );

  if( scattering_function_arg >=
      d_scattering_function->getUpperBoundOfIndepVar() )
    scattering_function_arg = d_scattering_function->getUpperBoundOfIndepVar();

  // Make sure the scattering function arg is valid
  testPostcondition( scattering_function_arg >=
		     d_scattering_function->getLowerBoundOfIndepVar() );
  testPostcondition( scattering_function_arg <=
		     d_scattering_function->getUpperBoundOfIndepVar() );

  return d_scattering_function->evaluate( scattering_function_arg );
}

// Evaluate the scattering function
template<typename ScatteringFunctionArgUnitConversionPolicy>
inline double WHIncoherentAdjointPhotonScatteringDistribution<ScatteringFunctionArgUnitConversionPolicy>::evaluateScatteringFunction(
				   const double incoming_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( incoming_energy <= this->getMaxEnergy() );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >=
		    calculateMinScatteringAngleCosine( incoming_energy,
						       this->getMaxEnergy() ));
  testPrecondition( scattering_angle_cosine <= 1.0 );

  return this->evaluateScatteringFunction( incoming_energy,
					   this->getMaxEnergy(),
					   scattering_angle_cosine );
}

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_WH_INCOHERENT_ADJOINT_PHOTON_SCATTERING_DISTRIBUTION_DEF_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_WHIncoherentAdjointPhotonScatteringDistribution.cpp
//---------------------------------------------------------------------------//


#include <cstddef>
#include <sstream>

#include <boost/multiprecision/cpp_int.hpp>

#include <locic/Support/APInt.hpp>

namespace locic{

	class APIntImpl {
	public:
		typedef boost::multiprecision::cpp_int ValueType;

		APIntImpl(const uint64_t argValue)
		: value_(argValue) { }

		APIntImpl(const APIntImpl&) = default;

		ValueType& value() {
			return value_;
		}

		const ValueType& value() const {
			return value_;
		}

	private:
		ValueType value_;

	};

	APInt::APInt()
	: impl_(nullptr) { }

	APInt::APInt(const uint64_t value)
	: impl_(new APIntImpl(value)) { }

	APInt::APInt(const APInt& other)
	: impl_(new APIntImpl(*(other.impl_))) { }

	APInt::APInt(APInt&& other)
	: impl_(std::move(other.impl_)) { }

	APInt& APInt::operator=(APInt&& other) {
		impl_ = std::move(other.impl_);
		return *this;
	}

	APInt::~APInt() { }

	APInt APInt::copy() const {
		return APInt(*this);
	}

	APInt& APInt::operator+=(const APInt& other) {
		impl_->value() += other.impl_->value();
		return *this;
	}

	APInt& APInt::operator*=(const APInt& other) {
		impl_->value() *= other.impl_->value();
		return *this;
	}

	bool APInt::operator==(const APInt& other) const {
		return impl_->value() == other.impl_->value();
	}

	bool APInt::operator!=(const APInt& other) const {
		return impl_->value() != other.impl_->value();
	}

	bool APInt::operator<(const APInt& other) const {
		return impl_->value() < other.impl_->value();
	}

	bool APInt::operator<=(const APInt& other) const {
		return impl_->value() <= other.impl_->value();
	}

	bool APInt::operator>(const APInt& other) const {
		return impl_->value() > other.impl_->value();
	}

	bool APInt::operator>=(const APInt& other) const {
		return impl_->value() >= other.impl_->value();
	}

	uint64_t APInt::asUint64() const {
		return static_cast<uint64_t>(impl_->value());
	}

	size_t APInt::hash() const {
		return static_cast<size_t>(impl_->value());
	}

	std::string APInt::toString() const {
		std::ostringstream stream;
		stream << impl_->value();
		return stream.str();
	}

}


#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/Twist.h>
#include "pid.hpp"
//#include <Eigen/Geometry>
// #include <Eigen/Dense>
#include <cmath>

using namespace std;

struct Position
{
	float x,y,z;
	float oX,oY,oZ;
	Position ()
	{
		x=y=z=oX=oY=0;
		oZ=0;
	}
	Position subtract (Position Q)
	{
		Position P;
		P.x= x-Q.x;
		P.y= y-Q.y;
		P.z= z-Q.z;
		return P;
	}
	void add (Position Q)
	{
		x= x+Q.x;
		y= y+Q.y;
		z= z+Q.z;
	}
		void divide (int i)
	{
		x= x/i;
		y= y/i;
		z= z/i;
	}
};

class Graph {
	private:
	    enum State {
	        Idle = 0,
	        Automatic = 1,
	        TakingOff = 2,
	        Landing = 3,
	    };

		vector<Position> poseList;
		vector <vector<int> > edge;
		vector<vector<Position> > formation;
		vector<geometry_msgs::PoseStamped> goals;
		vector<ros::Publisher> publish_goals;
		int count;
		std::string m_worldFrame;
    	vector<std::string> m_frame;
		tf::TransformListener m_listener;

		void updatePosition() {
			for(int i=0;i<count;i++) {
				tf::StampedTransform transform;
                m_listener.lookupTransform(m_worldFrame, m_frame.at(i), ros::Time(0), transform);
                poseList.at(i).x=transform.getOrigin().x();
                poseList.at(i).y=transform.getOrigin().y();
                poseList.at(i).z=transform.getOrigin().z();
			}
		}

	 	void iteration(const ros::TimerEvent& e){
	        computeNewPosition();
	        // cout<<"-------------- NO ERROR YET --------------"<<endl;
	    }

		void insertNode(int i) {
			//Henneberg construction
			//count++;
			Position P;
			//vector<int> (edge.at(i))();										/////////////////////
			poseList.push_back(P);
			if(i==1)
				edge.at(1).push_back(0);
			else if(i<=5) {
				edge.at(i).push_back(i-2);
				edge.at(i).push_back(i-1);
			}
			else {
				edge.at(i).push_back(i-3);
				edge.at(i).push_back(i-2);
				edge.at(i).push_back(i-1);
			}
		}

		void getFormation(string filename) {
			string line;
			ifstream myfile("/home/abhinav/crazyflie_ws/src/crazyflie_ros/crazyflie_controller/src/formation.txt");
			if(myfile.is_open()) {
				formation.resize(count);
				for(int i=0;i<count;i++) {
				 	formation.at(i).resize(count);
				 	for(int j=0;j<count;j++) {
				 		float f;
				 		myfile>>f;
				 		// cout<<f<<endl;
				 		formation.at(i).at(j).x=f;
				 	}
				}
				for(int i=0;i<count;i++) {
				 	for(int j=0;j<count;j++) {
				 		float f;
				 		myfile>>f;
				 		// cout<<f<<endl;
				 		formation.at(i).at(j).y=f;
				 	}
				}
				for(int i=0;i<count;i++) {
				 	for(int j=0;j<count;j++) {
				 		float f;
				 		myfile>>f;
				 		// cout<<f<<endl;
				 		formation.at(i).at(j).z=f;
				 	}
				}
				myfile.close();
			}
		}

		void computeNewPosition() {
			updatePosition();
			for(int i=1;i<count;i++) {
				Position current=getPosition(i);
				Position desired;
				for(int j=0;j<edge.at(i).size();j++) {
					int k= edge.at(i).at(j);
					Position p=getPosition(k);
					desired.add(p.subtract(formation.at(i).at(j)));
 				}
 				desired.divide(edge.at(i).size());
				goals.at(i).header.seq += 1;
				goals.at(i).header.stamp = ros::Time::now();
				goals.at(i).pose.position.x = desired.x;
				goals.at(i).pose.position.y = desired.y;
				goals.at(i).pose.position.z = desired.z;

				cout<<"No error yet"<<endl;

				tf::Quaternion q = tf::createQuaternionFromRPY(0, 0, 0);
				q=q.normalized();
				for(int i=0;i<4;i++)
				std::cout<<q[i]<<endl;

				goals.at(i).pose.orientation.x = 0;
				goals.at(i).pose.orientation.y = 0;//q[1];
				goals.at(i).pose.orientation.z = 0;//q[2];
				goals.at(i).pose.orientation.w = 1;//q[3];

	            publish_goals.at(i).publish(goals.at(i));
                ////////////// Give desired position to each node somehow?????
			}
		}

	public:
		Graph(){}
		Graph(std::string worldFrame,int frame) {
			count=frame;
			m_worldFrame=worldFrame;
			m_frame.resize(count);
			ros::NodeHandle nh;
			//m_listener.resize(count); /////////////////////////////////////////
			edge.resize(count);
			vector<vector<int> > edge(count,vector<int>());   /////////////////////////
			goals.resize(count);
			publish_goals.resize(count);
			for(int i=0;i<count;i++) {
				m_frame.at(i)="crazyflie"+std::to_string(i);
				m_listener.waitForTransform(m_worldFrame,m_frame.at(i),ros::Time(0),ros::Duration(10.0));
				insertNode(i);
				goals.at(i).header.seq=0;
				goals.at(i).header.frame_id=m_worldFrame;
				if(i != 0){
					publish_goals.at(i)=nh.advertise<geometry_msgs::PoseStamped>(m_frame.at(i)+"/goal",1); 			// m_frame.at(i)+"/goal"
				}
			}
			this->getFormation("formation.txt");
		}

		Position getPosition(int i) {
			return poseList.at(i);
		}

	    void run(double frequency) {
	        ros::NodeHandle node;
	        ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &Graph::iteration, this);
	        ros::spin();
	    }
};

int main(int argc, char **argv)
{
	cout<<"-------------------------------------Graph started running-------------------------------------"<<endl<<endl;
	ros::init(argc, argv, "graph");

	// Read parameters
	ros::NodeHandle n("~");
	std::string worldFrame;
	n.param<std::string>("worldFrame", worldFrame, "/world");
	int frame;
	n.getParam("frame", frame);
	double frequency;
	n.param("frequency", frequency, 50.0);
	Graph graph(worldFrame,frame);
	graph.run(frequency);

	return 0;
}


#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/ncs/tncs.h>
#include <boost/python/return_value_policy.hpp>
#include <boost/python.hpp>

namespace mmtbx { namespace ncs {
  namespace bp = boost::python;

namespace {
  boost::python::tuple
  getinitargs(pair<> const& self)
  {
    return boost::python::make_tuple(self.r, self.t);
  }

  using namespace boost::python;
  using boost::python::arg;

  template <typename FloatType>
  struct tncs_eps_factor_refinery_wrapper
  {
    typedef tncs_eps_factor_refinery<FloatType> w_t;
    static void wrap() {
      using namespace boost::python;
      class_<w_t>("tncs_eps_factor_refinery", no_init)
        .def(init<
             bp::list const&,
             af::const_ref<double> const&,
             af::const_ref<double> const&,
             af::const_ref<int> const&,
             af::const_ref<double> const&,
             cctbx::sgtbx::space_group,
             af::const_ref<cctbx::miller::index<int> >,
             scitbx::mat3<double>,
             af::shared<scitbx::mat3<double> >
                  >((arg("tncs_pairs"),
                     arg("f_obs"),
                     arg("sigma_f_obs"),
                     arg("rbin"),
                     arg("SigmaN"),
                     arg("space_group"),
                     arg("miller_indices"),
                     arg("fractionalization_matrix"),
                     arg("sym_matrices"))))
        .def("tncs_epsfac", &w_t::tncs_epsfac_result)
        .def("target", &w_t::target_gradient)
        .def("gradient_rhoMN", &w_t::gradient_rhoMN)
        .def("gradient_radius", &w_t::gradient_radius)
        .def("update_pairs", &w_t::update_pairs)
        .def("set_compute_gradients_rho_mn", &w_t::set_compute_gradients_rho_mn)
        .def("set_compute_gradients_radius", &w_t::set_compute_gradients_radius)
      ;
    }
  };

  void init_module()
  {
    tncs_eps_factor_refinery_wrapper<double>::wrap();

    typedef return_value_policy<return_by_value> rbv;
    class_<pair<> >("pair")
      .def(init<scitbx::mat3<double> const&,
                scitbx::vec3<double> const&,
                double,
                double,
                double,
                af::shared<double>,
                int >((
                  arg("r"),
                  arg("t"),
                  arg("radius"),
                  arg("radius_estimate"),
                  arg("fracscat"),
                  arg("rho_mn"),
                  arg("id"))))
      .add_property("r",      make_getter(&pair<>::r,      rbv()))
      .add_property("t",      make_getter(&pair<>::t,      rbv()))
      .add_property("radius", make_getter(&pair<>::radius, rbv()))
      .add_property("radius_estimate", make_getter(&pair<>::radius_estimate, rbv()))
      .add_property("fracscat", make_getter(&pair<>::fracscat, rbv()))
      .add_property("rho_mn", make_getter(&pair<>::rho_mn, rbv()))
      .add_property("id", make_getter(&pair<>::id, rbv()))
      .enable_pickling()
      .def("__getinitargs__", getinitargs)
      .def("set_radius", &pair<>::set_radius)
      .def("set_rhoMN", &pair<>::set_rhoMN)
      .def("set_id", &pair<>::set_id)
    ;

  }

} // namespace <anonymous>
}} // namespace mmtbx::ncs

BOOST_PYTHON_MODULE(mmtbx_ncs_ext)
{
  mmtbx::ncs::init_module();
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSSE3_SIMD_FUNCTION_BROADCAST_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSSE3_SIMD_FUNCTION_BROADCAST_HPP_INCLUDED

#include <boost/simd/detail/nsm.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/dispatch/adapted/std/integral_constant.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  BOOST_DISPATCH_OVERLOAD ( broadcast_
                          , (typename A0, typename A1)
                          , bs::ssse3_
                          , bs::pack_<bd::ints8_<A0>,bs::sse_>
                          , bd::constant_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      return do_(a0,nsm::bool_<(A1::value >= 0 && A1::value < 16)>{});
    }

    static BOOST_FORCEINLINE A0 do_(A0 const& a0, nsm::bool_<true> const&) BOOST_NOEXCEPT
    {
      return _mm_shuffle_epi8(a0, _mm_set1_epi8(A1::value) );
    }

    static BOOST_FORCEINLINE A0 do_(A0 const&, nsm::bool_<false> const&) BOOST_NOEXCEPT
    {
      return Zero<A0>();
    }
  };
} } }

#endif


/**
\file SBGATObs.hpp
\author Benjamin Bercovici
\author Jay McMahon
\date October 2018
\brief Defines the SBGATObs class
\details Defines the SBGATObs that is the base class for synthetic observation generations.
This class is responsible for shape model pre-processing used.
\copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATOBS_HEADER
#define SBGATOBS_HEADER

#include <vtkModifiedBSPTree.h>
#include <vtkPolyData.h>
#include <vtkPolyDataAlgorithm.h>
#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkInformation.h>
#include <vtkInformationVector.h>

#include <armadillo>

class VTKFILTERSCORE_EXPORT SBGATObs : public vtkPolyDataAlgorithm {

public:

	static SBGATObs * New();

	vtkTypeMacro(SBGATObs,vtkPolyDataAlgorithm);

	void PrintSelf(std::ostream& os, vtkIndent indent) override;
	void PrintHeader(std::ostream& os, vtkIndent indent) override;
	void PrintTrailer(std::ostream& os, vtkIndent indent) override;

	/**
 	Sets the scale factor to 1, indicative that the polydata has its coordinates expressed in meters (default)
 	*/
	void SetScaleMeters() { this -> scaleFactor = 1; }

	/**
	Sets the scale factor to 1000, indicative that the polydata has its coordinates expressed in kilometers
	*/
	void SetScaleKiloMeters() { this -> scaleFactor = 1000; }

	/**
	Return the number of considered bodies
	@return number of considered bodies
	*/
	int get_number_of_bodies() const {return this -> number_of_bodies;}

protected:

	SBGATObs();
	~SBGATObs() override;

	int RequestData(vtkInformation* request,
		vtkInformationVector** inputVector,
		vtkInformationVector* outputVector) override;

	int FillInputPortInformation( int port, vtkInformation* info ) VTK_OVERRIDE;

	/*
	Determines what facets of a given shape satisfy a set of visibility conditions, formulated in terms of a
	direction that must point above the facet's horizon. Does not check for intersects and shadowing,
	only compares the outbound normal orientation to the unit vector direction of each condition. If
	a facet is found to be visible (i.e all conditions are satisfied),
	its index is added to facets_in_view.
	@param facets_in_view reference to vector holding indices of (maybe) illuminated facets
	@param body_index index of considered body
	@param dir_to_check_vec vector of directions (unit vectors) that must point above a facet's horizon for a facet to be considered as visible
	@param BN_dcms_vec vector holding the DCMs orienting the body frame of each body w/r to inertial
	@param positions_vec vector holding the position vector of the CM of each body w/r to the primary
	*/
	void prefind_facets_inview(
		std::vector<int> & facets_in_view,
		const unsigned int & body_index,
		const std::vector<arma::vec> & dir_to_check_vec,
		const std::vector<arma::mat> & BN_dcms_vec,
		const std::vector<arma::vec> & positions_vec);

	/**
	Computes the largest facet surface area of all of the considered shapes
	@return surface area of largest facet in all of the considered shapes
	*/
	void find_min_facet_surface_area();

	/**
	Checks if the line spanned between provided points intersects
	with any of the considered bodies.
	@param origin_body_index index of the body from which originates the line to be tested for intersect
	@param start_point_origin_body coordinates of the origin of the line, expressed in the original body frame
	@param end_point_inertial coordinates of the end of the line, expressed in the inertial frame
	@param BN_dcms_vec vector holding the DCMs orienting the body frame of each body w/r to inertial
	@param positions_vec vector holding the position vector of the CM of each body w/r to the primary
	@return true if the line intersects with any of the considered bodies, false otherwise
	*/
	bool check_line_for_intersect(const int & origin_body_index,
		const arma::vec & start_point_origin_body,
		const arma::vec & end_point_inertial,
		const std::vector<arma::mat> & BN_dcms_vec,
		const std::vector<arma::vec> & positions_vec,
		const double & tol) const;

	std::vector<vtkSmartPointer<vtkModifiedBSPTree>> bspTree_vec;
	std::vector<vtkPolyData *> polydata_vec;
	std::vector<arma::vec> center_of_mass_vec;

	double scaleFactor = 1;
	double min_area;
	int number_of_bodies;

private:
	SBGATObs(const SBGATObs&) = delete;
	void operator=(const SBGATObs&) = delete;

};

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_COMPARE_EQUAL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COMPARE_EQUAL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing compare_equal capabilities

    Returns a logical scalar value that is the result of the lexicographic
    test for equality of all elements of the entries,
    i.e. return @ref True if and only if all corresponding entries
    elements are equal.

    It is probably not what you wish. Have a look to @ref is_equal

    @par Semantic:

    For every parameters of type T0:

    @code
    logical<scalar_of_t<T0>> r = compare_equal(x,y);
    @endcode

    is similar to:

    @code
    logical<scalar_of_t<T0>> r = all(x == y)
    @endcode

    @par Alias:
    @c compare_eq

    @see is_equal

  **/
  const boost::dispatch::functor<tag::compare_equal_> compare_equal = {};
} }
#endif

#include <boost/simd/function/scalar/compare_equal.hpp>
#include <boost/simd/function/simd/compare_equal.hpp>

#endif


#include <fstream>
#include <vector>
#include "RegressionTreeNode.hpp"

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

namespace qi = boost::spirit::qi;
namespace phoenix = boost::phoenix;

struct Test_tuple
{
   int date;
   int store;
   int item;
   double unit_sales;
   int onpromotion;
   double oilprize;
   int holiday_type;
   int locale;
   int locale_id;
   int transferred;
   int transactions;
   int city;
   int state;
   int store_type;
   int cluster;
   int family;
   int itemclass;
   int perishable;
   Test_tuple(const std::string& tuple)
   {
         qi::phrase_parse(tuple.begin(),tuple.end(),
            qi::int_[phoenix::ref(date) = qi::_1]>>
            qi::int_[phoenix::ref(store) = qi::_1]>>
            qi::int_[phoenix::ref(item) = qi::_1]>>
            qi::double_[phoenix::ref(unit_sales) = qi::_1]>>
            qi::int_[phoenix::ref(onpromotion) = qi::_1]>>
            qi::double_[phoenix::ref(oilprize) = qi::_1]>>
            qi::int_[phoenix::ref(holiday_type) = qi::_1]>>
            qi::int_[phoenix::ref(locale) = qi::_1]>>
            qi::int_[phoenix::ref(locale_id) = qi::_1]>>
            qi::int_[phoenix::ref(transferred) = qi::_1]>>
            qi::int_[phoenix::ref(transactions) = qi::_1]>>
            qi::int_[phoenix::ref(city) = qi::_1]>>
            qi::int_[phoenix::ref(state) = qi::_1]>>
            qi::int_[phoenix::ref(store_type) = qi::_1]>>
            qi::int_[phoenix::ref(cluster) = qi::_1]>>
            qi::int_[phoenix::ref(family) = qi::_1]>>
            qi::int_[phoenix::ref(itemclass) = qi::_1]>>
            qi::int_[phoenix::ref(perishable) = qi::_1],
            '|');
   }
};

void loadTestDataset(std::vector<Test_tuple>& TestDataset)
{
   std::ifstream input;
   std::string line;
   input.open("/Users/yuhao/LMFAO/data/favorita-small/test_data.tbl");
   if (!input)
   {
      std::cerr << "TestDataset.tbl does is not exist.\n";
      exit(1);
   }
   while(getline(input, line))
      TestDataset.push_back(Test_tuple(line));
   input.close();
}

double findPrediction(const RegTreeNode* node, const Test_tuple& tuple)
{
   if (node->isLeaf)
      return node->prediction;

   bool goLeft = true;
   const Condition& c = node->condition;
   switch (c.variable)
   {
         case 0 : goLeft = (tuple.date <= c.threshold); break;
         case 1 : goLeft = (tuple.store == c.threshold); break;
         case 2 : goLeft = (tuple.item <= c.threshold); break;
         case 3 : goLeft = (tuple.unit_sales <= c.threshold); break;
         case 4 : goLeft = (tuple.onpromotion == c.threshold); break;
         case 5 : goLeft = (tuple.oilprize <= c.threshold); break;
         case 6 : goLeft = (tuple.holiday_type == c.threshold); break;
         case 7 : goLeft = (tuple.locale == c.threshold); break;
         case 8 : goLeft = (tuple.locale_id == c.threshold); break;
         case 9 : goLeft = (tuple.transferred == c.threshold); break;
         case 10 : goLeft = (tuple.transactions <= c.threshold); break;
         case 11 : goLeft = (tuple.city == c.threshold); break;
         case 12 : goLeft = (tuple.state == c.threshold); break;
         case 13 : goLeft = (tuple.store_type == c.threshold); break;
         case 14 : goLeft = (tuple.cluster == c.threshold); break;
         case 15 : goLeft = (tuple.family == c.threshold); break;
         case 16 : goLeft = (tuple.itemclass == c.threshold); break;
         case 17 : goLeft = (tuple.perishable == c.threshold); break;
   }
   if (goLeft)
      return findPrediction(node->lchild, tuple);
   else
      return findPrediction(node->rchild, tuple);
}

void evaluateModel(const RegTreeNode* root)
{
   std::vector<Test_tuple> TestDataset;
   loadTestDataset(TestDataset);
   double pred, diff, error = 0.0;
   for (const Test_tuple& tuple : TestDataset)
      {
      pred = findPrediction(root, tuple);
      diff = tuple.unit_sales-pred;
      error += diff * diff;
   }
   error /= TestDataset.size();
   std::cout << "RMSE: " << sqrt(error) << std::endl;
}

void generateDynamicFunctions(std::vector<Condition>& conditions)
{
   std::string variableMap[] = {"date","store","item","unit_sales","onpromotion","oilprize","holiday_type","locale","locale_id","transferred","transactions","city","state","store_type","cluster","family","itemclass","perishable"};
   std::string variableList[] = {"int date,int store,int item,double unit_sales,int onpromotion","int date,double oilprize","int date,int holiday_type,int locale,int locale_id,int transferred","int date,int store,int transactions","int store,int city,int state,int store_type,int cluster","int item,int family,int itemclass,int perishable"};
   size_t relationMap[] = {0,0,0,0,0,1,2,2,2,2,3,4,4,4,4,5,5,5};
   std::string relationName[] = {"Sales","Oil","Holiday","Transactions","Stores","Items"};
   std::string conditionStr[] = {"","","","","",""};
   for (Condition& c : conditions)
      conditionStr[relationMap[c.variable]] += variableMap[c.variable] + c.op + std::to_string(c.threshold) + "&&";
   std::ofstream ofs("DynamicFunctions.cpp", std::ofstream::out);
   ofs << "#include \"DynamicFunctions.h\"\nnamespace lmfao\n{\n";
   for (size_t rel = 0; rel < 6; ++rel)
   {
      if (!conditionStr[rel].empty())
      {
         conditionStr[rel].pop_back();
         conditionStr[rel].pop_back();
         ofs << "double f_"+relationName[rel]+"("+variableList[rel]+")\n{\n	return ("+conditionStr[rel]+" ? 1.0 : 0.0);\n}\n";
      }
      else
      {
         ofs << "double f_"+relationName[rel]+"("+variableList[rel]+")\n{\n\treturn 1.0;\n}\n";
      }
   }
   ofs << "}\n";
   ofs.close();
}


// Copyright 2020 Josh Pieper, jjp@pobox.com.  All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "base/leg_force.h"

#include <boost/test/auto_unit_test.hpp>

using mjmech::base::OptimizeLegForce;

BOOST_AUTO_TEST_CASE(LegForceTest, * boost::unit_test::tolerance(1e-3)) {
  {
    const auto result = OptimizeLegForce({
        {-1., -1.},
        {-1., 1.},
        {1., -1.},
        {1., 1.},
            });

    BOOST_TEST_REQUIRE(result.size() == 4);
    BOOST_TEST(result[0] == 0.25);
    BOOST_TEST(result[1] == 0.25);
    BOOST_TEST(result[2] == 0.25);
    BOOST_TEST(result[3] == 0.25);
  }

  {
    const auto result = OptimizeLegForce({{-4, 0}, {2, 0}});
    BOOST_TEST_REQUIRE(result.size() == 2);
    BOOST_TEST(result[0] == 0.3333);
    BOOST_TEST(result[1] == 0.6667);
  }
  {
    // Something that requires a simple balance.
    const auto result = OptimizeLegForce({{-4, 1}, {2, 1}});
    BOOST_TEST_REQUIRE(result.size() == 2);
    BOOST_TEST(result[0] == 0.3333);
    BOOST_TEST(result[1] == 0.6667);
  }

  {
    // No matter what we do, the Y axis will be off balance.
    const auto result = OptimizeLegForce({{-4, 1}, {2, 0}});
    BOOST_TEST_REQUIRE(result.size() == 2);
    BOOST_TEST(result[0] == 0.088235);
    BOOST_TEST(result[1] == 0.911764);
  }

  {
    // No matter what we do, the Y axis will be even more off balance.
    const auto result = OptimizeLegForce({{-4, 4}, {2, 0}});
    BOOST_TEST_REQUIRE(result.size() == 2);
    BOOST_TEST(result[0] == 0.007335);
    BOOST_TEST(result[1] == 0.992665);
  }
}


// morpho/cdl/bgl_expansions/algorithms/ullmann.hpp header file//
// Copyright (c) 2003-2008 Vladimir J. Sykora
// Copyright (c) 2007-2008 Vladimir J. Sykora and NCU Studies Ltd
// Modifications by Greg Landrum, January 2009
//
//*****************************************************************************
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

//*****************************************************************************

//-----------------------------------------------------------------------------

// Algorithm due to JR Ullmann. "An Algorithm for Subgraph
// Isomorphism". Journal of the Association for
// Computing Machinery, Vol 23, No.1, January 1976, pp 31-42.

#ifndef MORPHO_CDL_BGL_EXP_ULLMANN_HPP
#define MORPHO_CDL_BGL_EXP_ULLMANN_HPP

// -------- boost
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/graph/graph_traits.hpp>
// -------- std
#include <utility>  // for std::pair<>
#include <algorithm>

namespace boost {
namespace detail {

template <class Graph, class UblasMatrix, class BackInsertionSequence,
          class EdgeLabeling>
bool forward_checking(const Graph& g1, const Graph& g2, UblasMatrix& M,
                      size_t count, BackInsertionSequence& F,
                      size_t num_vert_g1, size_t num_vert_g2,
                      EdgeLabeling& edge_labeling) {
  typedef std::pair<typename graph_traits<Graph>::edge_descriptor, bool>
      edge_presence;
  typename BackInsertionSequence::iterator fi, fi_end = F.end();
  for (size_t k = count + 1; k < num_vert_g1; ++k) {
    for (size_t l = 0; l < num_vert_g2; ++l) {
      if (!M(k, l)) continue;
      // check mapping:
      fi = F.begin();
      while (fi != fi_end) {
        bool flag1(0), flag1_1(0), flag2(0), flag2_1(0);
        edge_presence ep1 = edge(k, fi->first, g1);
        if (ep1.second) {
          flag1_1 = true;
          edge_presence ep2 = edge(l, fi->second, g2);
          if (ep2.second) flag1 = edge_labeling(ep1.first, ep2.first);
        }
        if (flag1_1 && flag1) {
          M(k, l) = 1;
          ++fi;
          continue;
        }
        edge_presence ep2 = edge(l, fi->second, g2);
        if (ep2.second) {
          flag2_1 = true;
          ep1 = edge(k, fi->first, g1);
          if (ep1.second) {
            flag2 = edge_labeling(ep1.first, ep2.first);
          } else {
            // ring closed in main structure, not closed in query. This should
            // pass
            flag2 = true;
          }
        }
        if (flag2_1 && flag2) {  // if one edge exists, there must be a mapping
          M(k, l) = 1;
        } else if (!flag1_1 && !flag2_1) {  // or both edges are not present
          M(k, l) = 1;
        } else
          M(k, l) = 0;  // if not, there's no mapping
        ++fi;
      }
    }
  }
  // TODO: change the data structure of the M matrix to sparse matrix. This
  // wouldn't be necessary
  size_t cero_row(0);
  for (size_t k = 0; k < num_vert_g1; ++k) {
    for (size_t l = 0; l < num_vert_g2; ++l) {
      if (M(k, l))
        break;
      else
        ++cero_row;
    }
    if (cero_row == num_vert_g2) return false;  // if there is a cero row
    cero_row = 0;
  }
  return true;
}

template <class Graph, class EdgeLabeling, class UblasMatrix,
          class BackInsertionSequence>
bool backtrack(const Graph& g1, const Graph& g2, size_t count,
               const UblasMatrix& M, BackInsertionSequence& F,
               const size_t num_vert_g1, const size_t num_vert_g2,
               EdgeLabeling& edge_labeling) {
  if (count == num_vert_g1) return true;
  for (size_t i = 0; i < num_vert_g2; ++i) {
    if (M(count, i)) {
      F.push_back(std::make_pair(count, i));
      UblasMatrix M_prime(M);
      for (size_t m = count + 1; m < num_vert_g1; ++m) {
        M_prime(m, i) = 0;
      }

      if (forward_checking(g1, g2, M_prime, count, F, num_vert_g1, num_vert_g2,
                           edge_labeling)) {
        if (backtrack(g1, g2, count + 1, M_prime, F, num_vert_g1, num_vert_g2,
                      edge_labeling)) {
          return true;
        }
      }
      F.erase(std::remove(F.begin(), F.end(), std::make_pair(count, i)),
              F.end());
    }
  }
  return false;
}

template <class Graph, class EdgeLabeling, class UblasMatrix,
          class DoubleBackInsertionSequence>
void backtrack_all(const Graph& g1, const Graph& g2, size_t count,
                   const UblasMatrix& M, DoubleBackInsertionSequence& FF,
                   const size_t num_vert_g1, const size_t num_vert_g2,
                   EdgeLabeling& edge_labeling) {
  if (count == num_vert_g1) return;
  DoubleBackInsertionSequence holdFF;
  holdFF.insert(holdFF.begin(), FF.begin(), FF.end());
  FF.clear();
  for (size_t i = 0; i < num_vert_g2; ++i) {
    if (M(count, i)) {
      DoubleBackInsertionSequence tFF;

      UblasMatrix M_prime(M);
      for (size_t m = count + 1; m < num_vert_g1; ++m) {
        M_prime(m, i) = 0;
      }

      if (holdFF.size()) {
        for (typename DoubleBackInsertionSequence::const_iterator iter =
                 holdFF.begin();
             iter != holdFF.end(); ++iter) {
          typename DoubleBackInsertionSequence::value_type F = *iter;
          F.push_back(std::make_pair(count, i));
          if (forward_checking(g1, g2, M_prime, count, F, num_vert_g1,
                               num_vert_g2, edge_labeling)) {
            tFF.push_back(F);
          }
        }
      } else {
        typename DoubleBackInsertionSequence::value_type F;
        F.push_back(std::make_pair(count, i));
        if (forward_checking(g1, g2, M_prime, count, F, num_vert_g1,
                             num_vert_g2, edge_labeling)) {
          tFF.push_back(F);
        }
      }
      backtrack_all(g1, g2, count + 1, M_prime, tFF, num_vert_g1, num_vert_g2,
                    edge_labeling);
      if (tFF.size()) {
        for (typename DoubleBackInsertionSequence::const_iterator iter =
                 tFF.begin();
             iter != tFF.end(); ++iter) {
          FF.push_back(*iter);
        }
      }
    }
  }
}

template <class Graph, class VertexLabeling  // binary predicate
          ,
          class UblasMatrix>
void prepareM(const Graph& g1, const Graph& g2, VertexLabeling& vertex_labeling,
              UblasMatrix& M) {
  size_t rows(num_vertices(g1));
  size_t cols(num_vertices(g2));
  M.resize(rows, cols);
  // initialize the matrix:
  for (size_t i = 0; i < rows; ++i) {
    for (size_t j = 0; j < cols; ++j) {
      if (out_degree(j, g2) >= out_degree(i, g1) && vertex_labeling(i, j)) {
        M(i, j) = 1;
      } else
        M(i, j) = 0;
    }
  }
}
}  // namespace detail

// test if g1 is a subgraph of g2. mapped vertices are returned in F
// Mapping : first: g1 vertices, second : g2 vertices
// O( num_vertices(g1)! num_vertices(g1) ^ 3 )
// This function doesnt Work with filtered graphs!
// The size of F doesn't indicate match!
template <
    class Graph, class VertexLabeling  // binary predicate
    ,
    class EdgeLabeling  // binary predicate
    ,
    class
    BackInsertionSequence  // contains
                           // std::pair<vertex_descriptor,vertex_descriptor>
    >
bool ullmann(const Graph& g1, const Graph& g2, VertexLabeling& vertex_labeling,
             EdgeLabeling& edge_labeling, BackInsertionSequence& F) {
  typedef ::boost::numeric::ublas::matrix<int> matrix_t;
  size_t rows(num_vertices(g1));
  size_t cols(num_vertices(g2));
  matrix_t M;
  detail::prepareM(g1, g2, vertex_labeling, M);
  size_t count(0);
  return detail::backtrack(g1, g2, count, M, F, rows, cols, edge_labeling);
}

// test if g1 is a subgraph of g2.
// F returns all mappings of g1 in g2. mapping in separate containers
template <class Graph, class VertexLabeling  // binary predicate
          ,
          class EdgeLabeling  // binary predicate
          ,
          class DoubleBackInsertionSequence  // contains a back insertion
                                             // sequence
          >
bool ullmann_all(const Graph& g1, const Graph& g2,
                 VertexLabeling& vertex_labeling, EdgeLabeling& edge_labeling,
                 DoubleBackInsertionSequence& F) {
  typedef ::boost::numeric::ublas::matrix<int> matrix_t;
  size_t rows(num_vertices(g1));
  size_t cols(num_vertices(g2));
  matrix_t M;
  detail::prepareM(g1, g2, vertex_labeling, M);
  size_t count(0);
  detail::backtrack_all(g1, g2, count, M, F, rows, cols, edge_labeling);
  return !F.empty();
}

}  // namespace boost

#endif  // MORPHO_CDL_BGL_EXP_ULLMANN_HPP


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_KINEMATICS_FKTABLE_HPP
#define RW_KINEMATICS_FKTABLE_HPP

/**
 * @file FKTable.hpp
 */
#if !defined(SWIG)
#include "FrameMap.hpp"
#include "State.hpp"

#include <rw/math/Transform3D.hpp>
#endif
/*
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index_container.hpp>
*/

namespace rw { namespace kinematics {

    class Frame;

    /** @addtogroup kinematics */
    /*@{*/

    /**
     * @brief Forward kinematics for a set of frames.
     *
     * FKTable finds transforms for frames for a given fixed work cell state.
     * The frame transforms are calculated relative to the world frame.
     */
    class FKTable
    {
      public:
        /**
         * @brief Forward kinematics for the work cell state \b state.
         *
         * @param state [in] The work state for which world transforms are to be
         * calculated.
         */
        FKTable (const rw::kinematics::State& state);
#if !defined(SWIGPYTHON) && !defined(SWIGJAVA) && !defined(SWIGLUA)
        /**
         * @brief Forward kinematics for the work cell state \b state.
         * @param state [in] The work state for which world transforms are to be
         * calculated.
         */
        FKTable (const rw::kinematics::State* state);
#endif
        /**
         * @brief The world transform for the frame \b frame.
         *
         * @param frame [in] The frame for which to find the world transform.
         *
         * @return The transform of the frame relative to the world.
         */
        const rw::math::Transform3D<>& get (const rw::kinematics::Frame& frame) const;

        //! @copydoc get(const Frame&) const
        inline const rw::math::Transform3D<>& get (const rw::kinematics::Frame* frame) const { return get (*frame); }

        /**
         * @brief Returns State associated with the FKTable
         *
         * The State returned is the State used to calculate the forward kinematics.
         *
         * @return State used to calculate the forward kinematics
         */
        const rw::kinematics::State& getState () const { return *_sp; }

        /**
         * @brief resets the FKTable to \b state
         * @param state
         */
        void reset (const rw::kinematics::State& state);

      private:
        const State* _sp;
        State _state;

        typedef FrameMap< math::Transform3D<> > TransformMap;

        mutable TransformMap _transforms;

      private:
        FKTable (const FKTable&);
    };

    /*@}*/
}}    // namespace rw::kinematics

#endif    // end include guard