// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file gps_aligned_frame.cpp
 * \brief
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#if 0
#include <Eigen/Core>
#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H 1
#include <proj_api.h>
#endif

#include <vtr_pose_graph/relaxation/gps_aligned_frame.hpp>

namespace vtr {
namespace pose_graph {
#if 0
constexpr char GpsAlignedFrame::gpsStream[];

GpsAlignedFrame::GpsAlignedFrame(const RCGraphBase::Ptr &graph, IterType begin,
                                 bool lazy)
    : Base(begin, graph->end(), true, true),
      utmZone_(0),
      C_(Eigen::Matrix3d::Identity()),
      r_(0, 0, 0) {
  // We need to have a GPS stream to make this work; otherwise just leave it as
  // a normal expansion
  if (!graph->hasVertexStream(gpsStream)) {
    LOG(ERROR) << "[GpsAlignedFrame] Graph does not have GPS data!";
    return;
  }

  graph->loadVertexStream(gpsStream);
  std::map<VertexIdType, Eigen::Vector3d> coords;
  Eigen::Vector3d r_utm(0, 0, 0), r_local(0, 0, 0);

  // Load GPS data and find the mean for each vertex
  for (auto it = graph->beginVertex(), ite = graph->endVertex(); it != ite;
       ++it) {
    auto data = it->template retrieveData<GpsMsgType>(gpsStream);
    if (data.size() > 0) {
      Eigen::Vector3d mean(0, 0, 0);

      for (auto &&jt : data) {
        mean[0] += jt->latitude();
        mean[1] += jt->longitude();
        mean[2] += jt->altitude();
      }

      mean /= data.size();
      coords.emplace(it->id(), mean);
      r_utm += mean;

      // Also find the mean for the local expansion points
      r_local += tfMap_[it->id()].r_ba_ina();
    }
  }

  r_utm /= coords.size();
  r_local /= coords.size();

  std::string pstr(
      "+proj=utm +ellps=WGS84 +datum=WGS84 +units=m +no_defs +zone=");
  utmZone_ = uint32_t((r_utm(1) + 180.) / 6.) + 1;
  pstr += std::to_string(utmZone_);
  projPJ pj_utm;

  if (!(pj_utm = pj_init_plus(pstr.c_str()))) {
    LOG(ERROR) << "[GpsAlignedFrame] Could not build UTM projection";
    graph->unloadVertexStream(gpsStream);
    return;
  }

  // Copy the transform map and then reset it; we will replace what we can with
  // GPS data, and the rest will be re-expanded
  Base pf(begin, graph->end(), false);
  tfMap_.clear();

  // Convert the GPS centroid into UTM coordinates in [m]
  projUV src, res;
  src.u = r_utm(1) * DEG_TO_RAD;
  src.v = r_utm(0) * DEG_TO_RAD;
  res = pj_fwd(src, pj_utm);
  r_utm(0) = res.u;
  r_utm(1) = res.v;

  Eigen::Matrix4d N(Eigen::Matrix4d::Zero());

  for (auto &&it : coords) {
    // Project the GPS coordinates of each vertex in UTM coordinates in [m]
    src.u = it.second(1) * DEG_TO_RAD;
    src.v = it.second(0) * DEG_TO_RAD;
    res = pj_fwd(src, pj_utm);

    it.second(0) = res.u;
    it.second(1) = res.v;

    // Horn's method... just trust me
    Eigen::Matrix3d S =
        (pf[it.first].r_ba_ina() - r_local) * (it.second - r_utm).transpose();
    Eigen::Matrix4d Ni;
    Ni << (S(0, 0) + S(1, 1) + S(2, 2)), S(1, 2) - S(2, 1), S(2, 0) - S(0, 2),
        S(0, 1) - S(1, 0), S(1, 2) - S(2, 1), (S(0, 0) - S(1, 1) - S(2, 2)),
        S(0, 1) + S(1, 0), S(2, 0) + S(0, 2), S(2, 0) - S(0, 2),
        S(0, 1) + S(1, 0), (-S(0, 0) + S(1, 1) - S(2, 2)), S(1, 2) + S(2, 1),
        S(0, 1) - S(1, 0), S(2, 0) + S(0, 2), S(1, 2) + S(2, 1),
        (-S(0, 0) - S(1, 1) + S(2, 2));
    N += Ni;
  }

  // The quaternion is the eigenvector with largest eigenvale.  Eigen3 always
  // sorts eigenvalues in ascending order.
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> solver(
      N, Eigen::DecompositionOptions::ComputeEigenvectors);
  Eigen::Vector4d q = solver.eigenvectors().col(0);
  double min = solver.eigenvalues()(0);

  for (int i = 1; i < 4; ++i) {
    if (solver.eigenvalues()(i) < min) {
      min = solver.eigenvalues()(i);
      q = solver.eigenvectors().col(i);
    }
  }

  Eigen::Matrix3d C;

  // Convert to rotation matrix
  C << -q(2) * q(2) - q(3) * q(3), q(1) * q(2) - q(3) * q(0),
      q(1) * q(3) + q(2) * q(0), q(1) * q(2) + q(3) * q(0),
      -q(1) * q(1) - q(3) * q(3), q(2) * q(3) - q(1) * q(0),
      q(1) * q(3) - q(2) * q(0), q(2) * q(3) + q(1) * q(0),
      -q(1) * q(1) - q(2) * q(2);
  C_ = (Eigen::Matrix3d::Identity() + 2 * C).transpose();

  // Offset from GPS frame to local frame
  Eigen::Vector3d r_ = C_.transpose() * r_utm - r_local;

  TransformType T(C_, r_);

  for (auto &&it : coords) {
    tfMap_.emplace(
        it.first,
        TransformType(Eigen::Matrix3d(pf[it.first].C_ba() * C_), it.second));
  }

  iter_ = ++graph->begin(tfMap_.begin()->first);

  if (!lazy) {
    computeAll();
  }

  graph->unloadVertexStream(gpsStream);
}
#endif
}  // namespace pose_graph
}  // namespace vtr


// Copyright 2021 RoboJackets
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// 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 AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <nav2_core/controller.hpp>
#include <pluginlib/class_list_macros.hpp>
#include <tf2_eigen/tf2_eigen.h>
#include <Eigen/Dense>
#include <angles/angles.h>
#include <vector>
#include <memory>
#include <string>
#include <algorithm>

namespace lqr_control
{

Eigen::Vector3d StateFromMsg(const geometry_msgs::msg::PoseStamped & pose)
{
  Eigen::Quaterniond orientation;
  tf2::fromMsg(pose.pose.orientation, orientation);
  Eigen::Vector3d state;
  state << pose.pose.position.x, pose.pose.position.y,
    orientation.toRotationMatrix().eulerAngles(0, 1, 2)[2];
  return state;
}

class LqrController : public nav2_core::Controller
{
public:
  void configure(
    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
    std::string name,
    const std::shared_ptr<tf2_ros::Buffer> & tf_buffer,
    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap) override
  {
    node_ = node;
    traj_viz_pub_ = node_->create_publisher<nav_msgs::msg::Path>(
      "~/tracking_traj",
      rclcpp::SystemDefaultsQoS());

    // BEGIN STUDENT CODE
    // END STUDENT CODE
  }

  Eigen::Vector3d interpolateState(const rclcpp::Time time)
  {
    if (time.seconds() > path_start_time_.seconds() + (time_between_states_ * trajectory_.size())) {
      return trajectory_.back();
    }
    if (time < path_start_time_) {
      return trajectory_.front();
    }

    double rel_time = (time - path_start_time_).seconds();
    int lower_idx = std::floor(rel_time / time_between_states_);
    int upper_idx = lower_idx + 1;
    double alpha = (rel_time - lower_idx * time_between_states_) / time_between_states_;

    Eigen::Vector3d interpolated = (1 - alpha) * trajectory_[lower_idx] + alpha *
      trajectory_[upper_idx];

    // correct the angle average
    if ((trajectory_[lower_idx](2) > 0 && trajectory_[upper_idx](2) < 0 ||
      trajectory_[lower_idx](2) < 0 && trajectory_[upper_idx](2) > 0) &&
      (std::abs(trajectory_[lower_idx](2)) > M_PI_2 && std::abs(
        trajectory_[upper_idx](2)) > M_PI_2))
    {
      double angle_diff = angles::shortest_angular_distance(
        trajectory_[lower_idx](2), trajectory_[upper_idx](2));
      interpolated(2) = trajectory_[lower_idx](2) + alpha * angle_diff;
      interpolated(2) = angles::normalize_angle(interpolated(2));
    }

    return interpolated;
  }

  void activate() override
  {
    traj_viz_pub_->on_activate();
  }

  void deactivate() override {}

  void cleanup() override {}

  void setPlan(const nav_msgs::msg::Path & path) override
  {
    trajectory_.clear();
    std::transform(
      path.poses.begin(), path.poses.end(),
      std::back_inserter(trajectory_), StateFromMsg);
    path_start_time_ = node_->now();
  }

  geometry_msgs::msg::TwistStamped computeVelocityCommands(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::Twist & velocity) override
  {
    Eigen::Vector3d state = StateFromMsg(pose);

    for (int i = 0; i < iterations_; i++) {
      computeRicattiEquation();
      computeForwardPass(state, pose.header.stamp);
    }

    geometry_msgs::msg::TwistStamped cmd_vel_msg;
    cmd_vel_msg.twist.linear.x = prev_u_[0](0);
    cmd_vel_msg.twist.angular.z = prev_u_[0](1);
    cmd_vel_msg.header.frame_id = "base_link";
    cmd_vel_msg.header.stamp = node_->now();
    return cmd_vel_msg;
  }

  Eigen::Matrix3d computeAMatrix(const Eigen::Vector3d & x, const Eigen::Vector2d & u)
  {
    // BEGIN STUDENT CODE
    return Eigen::Matrix3d::Zero();
    // END STUDENT CODE
  }

  Eigen::Matrix<double, 3, 2> computeBMatrix(const Eigen::Vector3d & x)
  {
    // BEGIN STUDENT CODE
    return Eigen::Matrix<double, 3, 2>::Zero();
    // END STUDENT CODE
  }

  Eigen::Vector3d computeNextState(const Eigen::Vector3d x, const Eigen::Vector2d u)
  {
    // BEGIN STUDENT CODE
    return x;
    // END STUDENT CODE
  }

  void computeRicattiEquation()
  {
    // does the backwards pass of the ricatti equation
    S_[S_.size() - 1] = Qf_;
    for (int t = T_ / dt_ - 2; t >= 0; t--) {
      auto last_S = S_[t + 1];
      Eigen::Matrix3d A = computeAMatrix(prev_x_[t], prev_u_[t]);
      Eigen::Matrix<double, 3, 2> B = computeBMatrix(prev_x_[t]);
      auto K = (R_ + B.transpose() * last_S * B).inverse() * B.transpose() * last_S * A;
      S_[t] = A.transpose() * last_S * A - (A.transpose() * last_S * B) * K + Q_;
    }
  }

  void computeForwardPass(const Eigen::Vector3d & init_x, rclcpp::Time current_time)
  {
    // computes the forward pass to update the states and controls
    Eigen::Vector3d cur_x = init_x;
    for (int t = 0; t < T_ / dt_; t++) {
      Eigen::Matrix3d A = computeAMatrix(cur_x, prev_u_[t]);
      Eigen::Matrix<double, 3, 2> B = computeBMatrix(prev_x_[t]);
      auto current_S = S_[t];
      auto K = (R_ + B.transpose() * current_S * B).inverse() * B.transpose() * current_S * A;
      Eigen::Vector3d state_error = (cur_x -
        interpolateState(current_time + rclcpp::Duration(dt_ * t * 1e9)));
      state_error(2) = angles::normalize_angle(state_error(2));
      Eigen::Vector2d u_star = -K * state_error;

      prev_x_[t] = cur_x;
      prev_u_[t] = u_star;

      cur_x = computeNextState(cur_x, u_star);
    }

    nav_msgs::msg::Path path;
    path.header.stamp = current_time;
    path.header.frame_id = "/map";
    for (const Eigen::Vector3d current_state : prev_x_) {
      geometry_msgs::msg::PoseStamped pose;
      pose.pose.position.x = current_state(0);
      pose.pose.position.y = current_state(1);
      path.poses.push_back(pose);
    }
    traj_viz_pub_->publish(path);
  }

private:
  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
  // dynamics
  double dt_;
  double T_;

  // cost function
  Eigen::Matrix3d Q_ = Eigen::Matrix3d::Identity();
  Eigen::Matrix3d Qf_ = Eigen::Matrix3d::Identity();
  Eigen::Matrix2d R_ = Eigen::Matrix2d::Identity();

  // Ricatti equation
  std::vector<Eigen::Matrix3d> S_;
  std::vector<Eigen::Vector3d> prev_x_;
  std::vector<Eigen::Vector2d> prev_u_;
  int iterations_;

  // trajectory to track
  std::vector<Eigen::Vector3d> trajectory_;
  double time_between_states_;
  rclcpp::Time path_start_time_;
  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr traj_viz_pub_;
};

}  // namespace lqr_control

PLUGINLIB_EXPORT_CLASS(lqr_control::LqrController, nav2_core::Controller)


/// @file  embed_cmp.cpp
/// @brief Embeds an object collection using oracle comparisons.

#include <boost/program_options.hpp>
#include <ogt/ogt.hpp>
#include <algorithm>
#include <chrono>
#include <map>

using Eigen::Index;
using Eigen::MatrixXd;
using namespace boost::program_options;
using nlohmann::json;
using OGT_NAMESPACE::core::CmpOutcome;
using OGT_NAMESPACE::embed::CmpConstraint;
using OGT_NAMESPACE::embed::EmbedConfig;
using OGT_NAMESPACE::embed::EmbedResult;
using OGT_NAMESPACE::io::loadCsvMatrix;
using OGT_NAMESPACE::io::openSink;
using OGT_NAMESPACE::io::openSource;
using OGT_NAMESPACE::io::saveCsvMatrix;
using OGT_NAMESPACE::io::saveEmbedding;
using OGT_NAMESPACE::linalg::embeddingFromKernelSVD;
using std::cerr;
using std::cout;
using std::endl;
using std::map;
using std::max;
using std::string;
using std::stringstream;
using std::vector;

/// Embed to a kernel using CK
const string METHOD_CK_K("ck_k");

/// Embed using CK
const string METHOD_CK_X("ck_x");

/// Embed to a kernel using GNMDS
const string METHOD_GNMDS_K("gnmds_k");

/// Embed using GNMDS
const string METHOD_GNMDS_X("gnmds_x");

/// Embed using Soft Ordinal Embedding
const string METHOD_SOE("soe");

/// Embed to a kernel using STE
const string METHOD_STE_K("ste_k");

/// Embed using STE
const string METHOD_STE_X("ste_x");

/// Embed using t-STE
const string METHOD_TSTE("tste");

/// Embed using PGD-X
const string METHOD_PGD_X("pgd_x");

/// Embed using PGD-K
const string METHOD_PGD_K("pgd_k");

/// Embed using NNPGD
const string METHOD_NNPGD("nnpgd");

/// Embed using NNPGD-debiased
const string METHOD_NNPGD_DEBIASED("nnpgd_debiased");

/// Embed using tau-for-k method
const string METHOD_TAU_K("tau_k");

/// Main program.
int run_embed_cmp(
	string cmp,
	size_t nDim,
	string method,
	string initPos,
	double margin,
	double lambda,
	double alpha,
	double minDelta,
	size_t maxIter,
	size_t bestOf,
	bool saveAll,
	double bestTol,
	bool verbose,
	string out,
	string outk,
	string outstats) {
	json results;
	results["args"] = {
		{"cmp", cmp},
		{"nDim", nDim},
		{"method", method},
		{"initPos", initPos},
		{"margin", margin},
		{"lambda", lambda},
		{"alpha", alpha},
		{"minDelta", minDelta},
		{"maxIter", maxIter},
		{"bestOf", bestOf},
		{"saveAll", saveAll},
		{"bestTol", bestTol},
		{"verbose", verbose},
		{"out", out},
		{"outk", outk},
		{"outstats", outstats},
	};
	EmbedConfig config;
	config.nDim = nDim;
	config.minDelta = minDelta;
	config.maxIter = maxIter;
	config.verbose = verbose;
	config.margin = margin;

	// Load the triples
	vector<CmpConstraint> cons;
	auto incmp = openSource(cmp);
	if (!incmp || !*incmp) {
		cerr << "Could not load constraints from: " << cmp << endl;
		return 1;
	}
	CmpOutcome con;
	map<size_t,size_t> points; // Map from true ID to temp ID
	vector<size_t> pointIds; // Map from temp ID to true ID
	size_t maxObj = 0;
	while (*incmp >> con) {
		for (size_t pt : {con.a, con.b, con.c}) {
			auto it = points.find(pt);
			if (it == points.end()) {
				points[pt] = pointIds.size();
				pointIds.push_back(pt);
				maxObj = max(maxObj, pt);
			}
		}
		cons.emplace_back(con);
	}
	size_t nObj = pointIds.size();
	cout << "Embedding " << nObj << " points referenced by " << cons.size() << " triples" << endl;
	results["args"]["nObj"] = nObj;
	results["num_triples"] = cons.size();

	// Rename objects as needed to embed only what we have constrained
	if (nObj < maxObj + 1) {
		for (auto& con : cons) {
			con.a = points[con.a];
			con.b = points[con.b];
			con.c = points[con.c];
			con.d = points[con.d];
		}
	} else {
		pointIds.clear();
	}

	// Load the initial positions, if we have an init file
	MatrixXd X0(nObj, nDim);
	if (!initPos.empty()) {
		try {
			auto X = loadCsvMatrix(initPos);
			if (pointIds.empty()) {
				X0 = X;
			} else {
				for (Index ii = 0; ii < X0.rows(); ii++) {
					X0.row(ii) = X.row(points[ii]);
				}
			}
		} catch (std::exception &e) {
			cerr << "Could not load initial positions: " << e.what() << endl;
			return 1;
		}
	}
	points.clear();

	auto save = [&](EmbedResult& res, string pathX, string pathK) {

		// Save the embedding
		bool gotKernel = (res.K.rows() > 0);
		try {
			if (gotKernel) {
				cout << "Calculating embedding from kernel..." << endl;
				res.X = embeddingFromKernelSVD(res.K, config.nDim);
			}
			if (!pointIds.empty()) {
				cout << "Writing " << pathX << endl;
				saveEmbedding(res.X, pointIds, pathX);
			} else {
				cout << "Writing " << pathX << endl;
				saveEmbedding(res.X, pathX);
			}
		} catch (const std::exception& e) {
			cerr << e.what() << endl;
			return;
		}

		// Save the kernel
		if (!pathK.empty()) {
			try {
				cout << "Writing " << pathK << endl;
				if (res.K.rows() == Eigen::Dynamic) {
					res.K = res.X * res.X.transpose();
				}
				saveCsvMatrix(res.K, pathK);
			} catch (const std::exception& e) {
				cerr << e.what() << endl;
				return;
			}
		}
	};

	// Produce the embedding
	EmbedResult bestRes;
	bestOf = max<size_t>(bestOf, 1);
	bestRes.loss = 1000 + bestTol;
	for (size_t rep = 0; rep < bestOf && (saveAll || bestRes.loss > bestTol); rep++) {
		if (initPos.empty()) {
			X0 = MatrixXd::Random(nObj, nDim);
		}

		cout << "Rep " << rep+1 << " / " << bestOf << ": Embedding " << nObj
			<< " objects into R^" << config.nDim << " using "
			<< cons.size() << " constraints" << endl;
		EmbedResult res;
		auto startTime = std::chrono::steady_clock::now();
		auto startProc = clock();
		if (method == METHOD_CK_K) {
			MatrixXd K0 = X0 * X0.transpose();
			res = embedCmpWithCKForK(cons, K0, lambda, config);
		} else if (method == METHOD_CK_X) {
			res = embedCmpWithCKForX(cons, X0, lambda, config);
		} else if (method == METHOD_GNMDS_K) {
			MatrixXd K0 = X0 * X0.transpose();
			res = embedCmpWithGNMDSForK(cons, K0, lambda, config);
		} else if (method == METHOD_GNMDS_X) {
			res = embedCmpWithGNMDSForX(cons, X0, lambda, config);
		} else if (method == METHOD_SOE) {
			res = embedCmpWithSOE(cons, X0, config);
		} else if (method == METHOD_STE_K) {
			MatrixXd K0 = X0 * X0.transpose();
			res = embedCmpWithSTEForK(cons, K0, lambda, config);
		} else if (method == METHOD_STE_X) {
			res = embedCmpWithSTEForX(cons, X0, lambda, config);
		} else if (method == METHOD_TSTE) {
			res = embedCmpWithTSTE(cons, X0, lambda, alpha, config);
		} else if (method == METHOD_PGD_X) {
			res = embedCmpWithPGDForX(cons, X0, config);
		} else if (method == METHOD_PGD_K) {
			MatrixXd K0 = X0 * X0.transpose();
			res = embedCmpWithPGDForK(cons, K0, config);
		} else if (method == METHOD_NNPGD) {
			MatrixXd K0 = X0 * X0.transpose();
			res = embedCmpWithNNPGD(cons, K0, lambda, config);
		} else if (method == METHOD_NNPGD_DEBIASED) {
			MatrixXd K0 = X0 * X0.transpose();
			res = embedCmpWithNNPGDDebiased(cons, K0, lambda, config);
		} else if (method == METHOD_TAU_K) {
			res = embedCmpWithTauForK(cons, lambda, 1 - lambda);
		} else {
			cerr << "Invalid embedding method \"" << method << "\"" << endl;
			return 1;
		}
		auto endTime = std::chrono::steady_clock::now();
		auto endProc = clock();
		std::chrono::duration<double> diffTime = (endTime - startTime);
		double durTime = diffTime.count();
		double durProc = (endProc - startProc) / CLOCKS_PER_SEC;

		cout << "Rep " << rep+1 << " / " << bestOf << ": Duration: " << durProc
			<< " CPU, " << durTime << " wall" << endl;
		cout << "Rep " << rep+1 << " / " << bestOf << ": Objective: "
			<< res.loss << endl;
		if (rep == 0 || bestRes.loss > res.loss) {
			bestRes = res;
			results["elapsed_perf_counter"] = durProc;
			results["elapsed_process_time"] = durTime;
			results["loss"] = res.loss;
		}

		if (saveAll) {
			stringstream outX, outK;
			outX << out << ".try_" << rep+1 << ".csv.gz";
			outK << outk << ".try_" << rep+1 << ".csv.gz";
			save(res, outX.str(), outK.str());
		}
	}
	save(bestRes, out, outk);

	// Save our statistics
	if (!outstats.empty()) {
		cout << "Writing " << outstats << endl;
		auto fstats = openSink(outstats);
		if (!fstats || !fstats->good()) {
			cerr << "Could not open statistics output file" << endl;
			return 1;
		}
		*fstats << std::setw(2) << results << endl;
	}

	cout << "Done" << endl;
	return 0;
}

/// Program entry point.
int main(int argc, char * argv[]) {
	try {
		string methodDesc = string("Embedding method: ")
			+ METHOD_CK_K + ", " + METHOD_CK_X + ", "
			+ METHOD_GNMDS_X + ", " + METHOD_GNMDS_K
		 	+ ", " + METHOD_SOE
		 	+ ", " + METHOD_STE_K + ", " + METHOD_STE_X + ", " + METHOD_TSTE
		 	+ ", " + METHOD_PGD_X + ", " + METHOD_PGD_K + ", " + METHOD_NNPGD
		 	+ ", " + METHOD_NNPGD_DEBIASED
		 	+ ", or " + METHOD_TAU_K;
		options_description desc{"embed_cmp options"};
		desc.add_options()
			("help,h", "Show this usage message")
			("cmp", value<string>(), "Path to a file containing comparisons to embed (*.csv)")
			("ndim", value<size_t>(), "Embedding dimensionality")
			("method", value<string>(), methodDesc.c_str())
			("init_pos", value<string>()->default_value(""), "Initialize from the specified positions instead of at random (*.csv)")
			("margin", value<double>()->default_value(0), "Constraint margin")
			("lambda", value<double>()->default_value(0), "Mixing parameter for loss functions")
			("alpha", value<double>()->default_value(0), "Degrees of freedom for loss functions")
			("min_delta", value<double>()->default_value(1e-12), "Min loss objective delta to stop")
			("max_iter", value<size_t>()->default_value(1000), "Max iteration count")
			("best_of", value<size_t>()->default_value(1), "Return best of n runs")
			("best_tol", value<double>()->default_value(0), "Stop when a run achieves this score")
			("save_all", value<bool>()->default_value(false), "Save intermediate embeddings")
			("verbose", value<bool>()->default_value(false), "Verbose output")
			("out", value<string>(), "Output path for the embedding (*.csv)")
			("outk", value<string>()->default_value(""), "Output path for the kernel (*.csv)")
			("outstats", value<string>()->default_value(""), "Output path for statistics (*.json)")
			;

		variables_map vm;
		store(parse_command_line(argc, argv, desc), vm);
		notify(vm);

		bool valid = true;
		if (vm.count("help")) {
			cout << desc << endl;
			valid = false;
		}
		if (!vm.count("cmp")) {
			cerr << "--cmp is required" << endl;
			valid = false;
		}
		if (!vm.count("ndim")) {
			cerr << "--ndim is required" << endl;
			valid = false;
		}
		if (!vm.count("method")) {
			cerr << "--method is required" << endl;
			valid = false;
		}
		if (!vm.count("out")) {
			cerr << "--out is required" << endl;
			valid = false;
		}
		if (valid) {
			return run_embed_cmp(
				vm["cmp"].as<string>(),
				vm["ndim"].as<size_t>(),
				vm["method"].as<string>(),
				vm["init_pos"].as<string>(),
				vm["margin"].as<double>(),
				vm["lambda"].as<double>(),
				vm["alpha"].as<double>(),
				vm["min_delta"].as<double>(),
				vm["max_iter"].as<size_t>(),
				vm["best_of"].as<size_t>(),
				vm["save_all"].as<bool>(),
				vm["best_tol"].as<double>(),
				vm["verbose"].as<bool>(),
				vm["out"].as<string>(),
				vm["outk"].as<string>(),
				vm["outstats"].as<string>());
		}
	} catch (const error &ex) {
		cerr << ex.what() << endl;
	}
	return 1;
}


#include <ros/ros.h>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <depth_image_proc/depth_conversions.h>
#include <visualization_msgs/Marker.h>
#include <cv_bridge/cv_bridge.h>
#include <boost/bind.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>

#include <mbzirc_comm_objs/ObjectDetectionList.h>
#include <mbzirc_comm_objs/ObjectDetection.h>

#include <iostream>

//static const std::string RECEIVED_IMAGE_WINDOW = "Depth image";
static const std::string CONVERTED_DEPTH_IMAGE_WINDOW = "Converted depth image";
static const std::string EDGES_IMAGE_WINDOW = "Edges image";
static const std::string HOUGH_LINES_IMAGE_WINDOW = "Hough Lines image";
static const std::string HOUGH_LINES_P_IMAGE_WINDOW = "Probabilistic Hough Lines image";
static const std::string CORNER_DETECTION_WINDOW = "Detected corners";

class WindowsDetector
{
public:
    WindowsDetector();
    ~WindowsDetector();
private:
    void depthCallback(const sensor_msgs::ImageConstPtr &image_ptr, const sensor_msgs::CameraInfoConstPtr &info_ptr);
    inline uint8_t mapFloatDepthImageToUInt8(float input);
    void detectWindows();
    void get3DPointCameraModel(geometry_msgs::Point &point, float &depth, int pixel_row, int pixel_col);
    void bubbleSort(std::vector<cv::Point2f>& vector_point_pair);
    void swapPairs(std::vector<cv::Point2f>& vector_point_pair, int index_1, int index_2);
    bool checkWindowSize(geometry_msgs::Vector3& windowDimensions);
    float calcDistanceBetween3DPoints(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2);
    geometry_msgs::Point calcWindowCenter(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2,
                                          geometry_msgs::Point& point_3, geometry_msgs::Point& point_4);
    geometry_msgs::Vector3 normalizedVectorFrom2Points(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2);
    geometry_msgs::Quaternion calcWindowOrientation(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2,
                                                    geometry_msgs::Point& point_3, geometry_msgs::Point& point_4);
    geometry_msgs::Vector3 normalizedCrossProduct(geometry_msgs::Vector3& vector_1, geometry_msgs::Vector3& vector_2);
    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;
    image_transport::ImageTransport image_transport_;
    image_transport::CameraSubscriber camera_sub_;
    image_transport::Publisher image_pub_;
    ros::Publisher corners_marker_pub_;
    ros::Publisher object_detection_pub_;
    ros::Publisher window_axis_markers_pub_;

    visualization_msgs::Marker corners_markers_msg_;
    visualization_msgs::Marker window_x_axis_marker_msg_;
    visualization_msgs::Marker window_y_axis_marker_msg_;
    visualization_msgs::Marker window_z_axis_marker_msg_;
    mbzirc_comm_objs::ObjectDetection object_detection_msg_;
    mbzirc_comm_objs::ObjectDetectionList object_detection_list_msg_;

    int image_counter_;

    image_geometry::PinholeCameraModel model_;

    cv_bridge::CvImage depth_image_8_;
    cv_bridge::CvImageConstPtr received_depth_image_opencv_ptr_;

    geometry_msgs::Point corner_upper_left_point_;
    geometry_msgs::Point corner_upper_right_point_;
    geometry_msgs::Point corner_lower_left_point_;
    geometry_msgs::Point corner_lower_right_point_;

    int maxCorners;
    cv::RNG rng;

    tf2_ros::TransformBroadcaster tf_broadcaster;
};

WindowsDetector::WindowsDetector():
image_transport_(nh_),
image_counter_(0),
rng(12345)
{
    image_transport::TransportHints hints("raw", ros::TransportHints(), ros::NodeHandle("~"));
    camera_sub_ = image_transport_.subscribeCamera("depth_image_topic", 1, &WindowsDetector::depthCallback, this, hints);
    image_pub_ = image_transport_.advertise("detected_lines_image", 1);
    corners_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("corners_markers", 1);
    object_detection_pub_ = nh_.advertise<mbzirc_comm_objs::ObjectDetectionList>("windows_detected", 1);
    window_axis_markers_pub_ = nh_.advertise<visualization_msgs::Marker>("window_axis", 1);

    // Corner markers
    corners_markers_msg_.header.frame_id = "uav_1";
    corners_markers_msg_.id = 1;
    corners_markers_msg_.lifetime = ros::Duration(0);
    corners_markers_msg_.ns = "corners";
    corners_markers_msg_.type = visualization_msgs::Marker::SPHERE_LIST;
    corners_markers_msg_.action = visualization_msgs::Marker::ADD;
    corners_markers_msg_.pose.orientation.w = 1.0;
    corners_markers_msg_.scale.x = corners_markers_msg_.scale.y = corners_markers_msg_.scale.z = 0.2;
    corners_markers_msg_.color.a = 1.0;

    std_msgs::ColorRGBA color_upper_left, color_upper_right, color_lower_left, color_lower_right;
    color_upper_left.a = color_upper_right.a = color_lower_left.a = color_lower_right.a = 1.0;
    color_upper_left.r = 1.0; // Upper left corner RED
    color_upper_right.g = 1.0; // Upper right corner GREEN
    color_lower_left.r = color_lower_left.g = 1.0; // Lower left corner YELLOW
    color_lower_right.b = 1.0; // Lower right corner BLUE

    corners_markers_msg_.colors.push_back(color_upper_left);
    corners_markers_msg_.colors.push_back(color_upper_right);
    corners_markers_msg_.colors.push_back(color_lower_left);
    corners_markers_msg_.colors.push_back(color_lower_right);

    // Window axis markers initialization
    window_x_axis_marker_msg_.header.frame_id = "uav_1";
    window_x_axis_marker_msg_.ns = "window_axis";
    window_x_axis_marker_msg_.id = 0;
    window_y_axis_marker_msg_.header.frame_id = "uav_1";
    window_x_axis_marker_msg_.ns = "window_axis";
    window_y_axis_marker_msg_.id = 1;
    window_z_axis_marker_msg_.header.frame_id = "uav_1";
    window_z_axis_marker_msg_.ns = "window_axis";
    window_z_axis_marker_msg_.id = 2;
    window_x_axis_marker_msg_.action = visualization_msgs::Marker::ADD;
    window_y_axis_marker_msg_.action = visualization_msgs::Marker::ADD;
    window_z_axis_marker_msg_.action = visualization_msgs::Marker::ADD;
    window_x_axis_marker_msg_.type = visualization_msgs::Marker::ARROW;
    window_y_axis_marker_msg_.type = visualization_msgs::Marker::ARROW;
    window_z_axis_marker_msg_.type = visualization_msgs::Marker::ARROW;
    window_x_axis_marker_msg_.scale.x = window_y_axis_marker_msg_.scale.x = window_z_axis_marker_msg_.scale.x = 0.01;
    window_x_axis_marker_msg_.scale.y = window_y_axis_marker_msg_.scale.y = window_z_axis_marker_msg_.scale.y = 0.02;
    window_x_axis_marker_msg_.color.a = window_y_axis_marker_msg_.color.a = window_z_axis_marker_msg_.color.a = 1.0;
    window_x_axis_marker_msg_.color.r = 1.0;
    window_y_axis_marker_msg_.color.g = 1.0;
    window_z_axis_marker_msg_.color.b = 1.0;

    // Object detection message initialization
    object_detection_msg_.header.frame_id = "uav_1";
    object_detection_msg_.type = object_detection_msg_.TYPE_PASSAGE;
    object_detection_msg_.color = object_detection_msg_.COLOR_UNKNOWN;

    // OpenCV Windows config
    //cv::namedWindow(RECEIVED_IMAGE_WINDOW, CV_WINDOW_AUTOSIZE);
    cv::namedWindow(CONVERTED_DEPTH_IMAGE_WINDOW, CV_WINDOW_AUTOSIZE);
    cv::namedWindow(EDGES_IMAGE_WINDOW, CV_WINDOW_AUTOSIZE);
    cv::namedWindow(HOUGH_LINES_IMAGE_WINDOW, CV_WINDOW_AUTOSIZE);
    cv::namedWindow(HOUGH_LINES_P_IMAGE_WINDOW, CV_WINDOW_AUTOSIZE);
    //cv::moveWindow(RECEIVED_IMAGE_WINDOW, 250, 250);
    cv::moveWindow(EDGES_IMAGE_WINDOW, 250, 250);
    cv::moveWindow(HOUGH_LINES_IMAGE_WINDOW, 250, 250);
    cv::moveWindow(HOUGH_LINES_P_IMAGE_WINDOW, 250, 250);

    // Corner detection test
    cv::namedWindow(CORNER_DETECTION_WINDOW, CV_WINDOW_AUTOSIZE);
    cv::moveWindow(CORNER_DETECTION_WINDOW, 250, 250);
}

WindowsDetector::~WindowsDetector()
{
    //cv::destroyWindow(RECEIVED_IMAGE_WINDOW);
    cv::destroyWindow(CONVERTED_DEPTH_IMAGE_WINDOW);
    cv::destroyWindow(EDGES_IMAGE_WINDOW);
    cv::destroyWindow(HOUGH_LINES_IMAGE_WINDOW);
    cv::destroyWindow(HOUGH_LINES_P_IMAGE_WINDOW);
}

void WindowsDetector::detectWindows()
{
    this->maxCorners = 4;

    /// Parameters for Shi-Tomasi algorithm
    std::vector<cv::Point2f> corners;
    double qualityLevel = 0.01;
    double minDistance = 10;
    int blockSize = 3;
    bool useHarrisDetector = false;
    double k = 0.04;

    cv::Mat copy;
    copy = this->depth_image_8_.image.clone();

    cv::goodFeaturesToTrack( this->depth_image_8_.image,
                             corners,
                             this->maxCorners,
                             qualityLevel,
                             minDistance,
                             cv::Mat(),
                             blockSize,
                             useHarrisDetector,
                             k );

    std::cout << "Number of corners detected: " << corners.size() << std::endl;
    int r = 4;
    for ( int i = 0; i < corners.size(); i++)
    {
        cv::circle( copy, corners[i], r, cv::Scalar(this->rng.uniform(0,255), this->rng.uniform(0,255), this->rng.uniform(0,255)), -1, 8, 0);
        //std::cout << "Corner " << i << " : (x,y)" << corners[i].x << "," << corners[i].y << std::endl;
    }
    imshow(CORNER_DETECTION_WINDOW, copy);

    if (corners.size() == 4)
    {
        const cv::Mat & original_image_ref = this->received_depth_image_opencv_ptr_->image;
        original_image_ref.data + original_image_ref.step[0];
        int row,col;

        // TODO: Order corners so they appear in the required position of the vector
        this->bubbleSort(corners);

        // TODO: Instead of calculating 3D position of corner, calc position of a near pixel, because corner has a NaN value depth.
        // The selection of the near pixel depends on the corner position (i.e. upper left, upper right, ...).
        float *  value_original_ptr;
        // First corner: Upper Left
        row = corners[0].y - 1;
        col = corners[0].x - 1;
        value_original_ptr = (float *) (original_image_ref.data + (original_image_ref.step[0]) * row + (original_image_ref.step[1])* col );
        float corner_upper_left_depth = *value_original_ptr;
        this->get3DPointCameraModel(corner_upper_left_point_, corner_upper_left_depth, row, col);

        // Second corner: Lower Left
        row = corners[1].y + 1;
        col = corners[1].x - 1;
        value_original_ptr = (float *) (original_image_ref.data + (original_image_ref.step[0]) * row + (original_image_ref.step[1])* col );
        float corner_lower_left_depth = *value_original_ptr;
        this->get3DPointCameraModel(corner_lower_left_point_, corner_lower_left_depth, row, col);

        // Third corner: Upper Right
        row = corners[2].y - 1;
        col = corners[2].x + 1;
        value_original_ptr = (float *) (original_image_ref.data + (original_image_ref.step[0]) * row + (original_image_ref.step[1])* col );
        float corner_upper_right_depth = *value_original_ptr;
        this->get3DPointCameraModel(corner_upper_right_point_, corner_upper_right_depth, row, col);

        // Fourth corner: Lower Right
        row = corners[3].y + 1;
        col = corners[3].x + 1;
        value_original_ptr = (float *) (original_image_ref.data + (original_image_ref.step[0]) * row + (original_image_ref.step[1])* col );
        float corner_lower_right_depth = *value_original_ptr;
        this->get3DPointCameraModel(corner_lower_right_point_, corner_lower_right_depth, row, col);

        geometry_msgs::Vector3 window_dimensions;
        if (this->checkWindowSize(window_dimensions))
        {
            this->corners_markers_msg_.points.push_back(corner_upper_left_point_);
            this->corners_markers_msg_.points.push_back(corner_upper_right_point_);
            this->corners_markers_msg_.points.push_back(corner_lower_left_point_);
            this->corners_markers_msg_.points.push_back(corner_lower_right_point_);
            this->corners_markers_msg_.header.stamp = ros::Time::now();
            this->corners_marker_pub_.publish(this->corners_markers_msg_);
            this->corners_markers_msg_.points.clear();

            geometry_msgs::Point window_center = this->calcWindowCenter(corner_upper_left_point_, corner_lower_left_point_,
                                                                        corner_upper_right_point_, corner_lower_right_point_);
            object_detection_msg_.pose.pose.position = window_center;
            geometry_msgs::Quaternion window_orientation = this->calcWindowOrientation(corner_upper_left_point_, corner_lower_left_point_,
                                                                                corner_upper_right_point_, corner_lower_right_point_);
            object_detection_msg_.pose.pose.orientation = window_orientation;
            object_detection_msg_.scale = window_dimensions;

            // TODO: Publish tf2 transformation between uav_1 frame and window frame for debugging
            geometry_msgs::TransformStamped transformStamped;
            transformStamped.header.stamp = ros::Time::now();
            transformStamped.header.frame_id = "uav_1";
            transformStamped.child_frame_id = "window";
            transformStamped.transform.rotation = window_orientation;
            transformStamped.transform.translation.x = window_center.x;
            transformStamped.transform.translation.y = window_center.y;
            transformStamped.transform.translation.z = window_center.z;
            tf_broadcaster.sendTransform(transformStamped);

            object_detection_list_msg_.objects.push_back(object_detection_msg_);
            this->object_detection_pub_.publish(object_detection_list_msg_);
            object_detection_list_msg_.objects.clear();
        }
    }
    else
    {
        std::cout << "Not enough corners detected" << std::endl;
    }

}

bool WindowsDetector::checkWindowSize(geometry_msgs::Vector3& window_dimensions)
{
    float distance_upper_left_upper_right = this->calcDistanceBetween3DPoints(this->corner_upper_left_point_, this->corner_upper_right_point_);
    float distance_lower_left_lower_right = this->calcDistanceBetween3DPoints(this->corner_lower_left_point_, this->corner_lower_right_point_);
    float distance_upper_left_lower_left = this->calcDistanceBetween3DPoints(this->corner_upper_left_point_, this->corner_lower_left_point_);
    float distance_upper_right_lower_right = this->calcDistanceBetween3DPoints(this->corner_upper_right_point_, this->corner_lower_right_point_);

    bool valid_distance_upper_left_upper_right = false;
    bool valid_distance_lower_left_lower_right = false;
    bool valid_distance_upper_left_lower_left = false;
    bool valid_distance_upper_right_lower_right = false;

    std::cout << "Distance between upper left and upper right corners: " << distance_upper_left_upper_right << std::endl;
    std::cout << "Distance between lower left and lower right corners: " << distance_lower_left_lower_right << std::endl;
    std::cout << "Distance between upper left and lower left corners: " << distance_upper_left_lower_left << std::endl;
    std::cout << "Distance between upper right and lower right corners: " << distance_upper_right_lower_right << std::endl;

    // Check window dimensions against a square of 2m x 2m
    if (distance_upper_left_upper_right > 1.9 && distance_upper_left_upper_right < 2.1)
    {
        valid_distance_upper_left_upper_right = true;
        window_dimensions.x = distance_upper_left_upper_right;
    }
    if (distance_lower_left_lower_right > 1.9 && distance_lower_left_lower_right < 2.1)
    {
        valid_distance_lower_left_lower_right = true;
    }
    if (distance_upper_left_lower_left > 1.9 && distance_upper_right_lower_right < 2.1)
    {
        valid_distance_upper_left_lower_left = true;
        window_dimensions.y = distance_upper_left_lower_left;
    }
    if (distance_upper_right_lower_right > 1.9 && distance_upper_right_lower_right < 2.1)
    {
        valid_distance_upper_right_lower_right = true;
    }

    // Check window dimensions against a rectangular window of 1.5m x 4m
    if (distance_upper_left_upper_right > 3.9 && distance_upper_left_upper_right < 4.1)
    {
        valid_distance_upper_left_upper_right = true;
        window_dimensions.x = distance_upper_left_upper_right;
    }
    if (distance_lower_left_lower_right > 3.9 && distance_lower_left_lower_right < 4.1)
    {
        valid_distance_lower_left_lower_right = true;
    }
    if (distance_upper_left_lower_left > 1.4 && distance_upper_right_lower_right < 1.6)
    {
        valid_distance_upper_left_lower_left = true;
        window_dimensions.y = distance_upper_left_lower_left;
    }
    if (distance_upper_right_lower_right > 1.4 && distance_upper_right_lower_right < 1.6)
    {
        valid_distance_upper_right_lower_right = true;
    }

    window_dimensions.z = 0;
    if (valid_distance_upper_left_upper_right && valid_distance_lower_left_lower_right && valid_distance_upper_left_lower_left && valid_distance_upper_right_lower_right)
    {
        std::cout << "Valid Window" << std::endl;
        return true;
    }
    else
    {
        std::cout << "Invalid Window" << std::endl;
        return false;
    }
}

geometry_msgs::Quaternion WindowsDetector::calcWindowOrientation(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2, geometry_msgs::Point& point_3, geometry_msgs::Point& point_4)
{
    tf2::Quaternion tf2_quaternion;
    /*
        point_1 -> upper left corner
        point_2 -> lower left corner
        point_3 -> upper right corner
        point_4 -> lower right corner
    */
    geometry_msgs::Quaternion orientation;

    // 1st Get x,y,z axis of the window reference frame
    // The axis orientation will be similar to how the camera frame axis are oriented.
    // z perpendicular to the plane containing the corners
    // x parallel to the vector defined by upper left corner and upper right corner
    // y parallel to the vector defined by upper left corner and lower left corner
    geometry_msgs::Vector3 x_axis_window, y_axis_window, z_axis_window;

    x_axis_window = this->normalizedVectorFrom2Points(point_1, point_3);
    y_axis_window = this->normalizedVectorFrom2Points(point_1, point_2);
    z_axis_window = this->normalizedCrossProduct(x_axis_window, y_axis_window);

    // Publish axis markers (arrows) for debugging
    /*
    geometry_msgs::Point window_center = calcWindowCenter(point_1,point_2,point_3,point_4);

    geometry_msgs::Point end_point_x_axis, end_point_y_axis, end_point_z_axis;

    end_point_x_axis.x = window_center.x + x_axis_window.x;
    end_point_x_axis.y = window_center.y + x_axis_window.y;
    end_point_x_axis.z = window_center.z + x_axis_window.z;

    end_point_y_axis.x = window_center.x + y_axis_window.x;
    end_point_y_axis.y = window_center.y + y_axis_window.y;
    end_point_y_axis.z = window_center.z + y_axis_window.z;

    end_point_z_axis.x = window_center.x + z_axis_window.x;
    end_point_z_axis.y = window_center.y + z_axis_window.y;
    end_point_z_axis.z = window_center.z + z_axis_window.z;

    window_x_axis_marker_msg_.points.push_back(window_center);
    window_x_axis_marker_msg_.points.push_back(end_point_x_axis);
    window_y_axis_marker_msg_.points.push_back(window_center);
    window_y_axis_marker_msg_.points.push_back(end_point_y_axis);
    window_z_axis_marker_msg_.points.push_back(window_center);
    window_z_axis_marker_msg_.points.push_back(end_point_z_axis);

    window_axis_markers_pub_.publish(window_x_axis_marker_msg_);
    window_axis_markers_pub_.publish(window_y_axis_marker_msg_);
    window_axis_markers_pub_.publish(window_z_axis_marker_msg_);
    window_x_axis_marker_msg_.points.clear();
    window_y_axis_marker_msg_.points.clear();
    window_z_axis_marker_msg_.points.clear();
    */

    // Get M matrix which is the matrix that allows us to obtain window reference frame axis from camera referenc frame axis
    //     |a11 a12 a13|
    // M = |a21 a22 a23|
    //     |a31 a32 a33|
    // aij is the dot product of the i vector of the transformed basis and the j vector of the original basis
    // As the original basis is composed of the normal vectors î,ĵ and k (^), the dot product is simplified
    // and the resulting matrix elements are just the components of the transformed basis vectors, arranged by rows
    // The first vector in the first row, second vector in second row, ...
    double a11, a12, a13, a21, a22, a23, a31, a32, a33;

    a11 = x_axis_window.x;
    a12 = x_axis_window.y;
    a13 = x_axis_window.z;

    a21 = y_axis_window.x;
    a22 = y_axis_window.y;
    a23 = y_axis_window.z;

    a31 = z_axis_window.x;
    a32 = z_axis_window.y;
    a33 = z_axis_window.z;

    // Get M transpose (Rotation matrix that allows us to transform coordinates expressed in transformed basis to original basis)
    //      |a11 a21 a31|   |r11 r12 r13|
    // Mt = |a12 a22 a32| = |r21 r22 r23|
    //      |a13 a23 a33|   |r31 r32 r33|

    // Get yaw, pitch and roll from M transpose
    double yaw = atan2(a12, a11);
    double pitch = atan2(-a13, sqrt(a23*a23+a33*a33));
    double roll = atan2(a23, a33);

    // Get quaternion from roll,pitch and yaw
    tf2_quaternion.setRPY(roll, pitch, yaw);
    tf2::convert(tf2_quaternion, orientation);
    return orientation;
}

geometry_msgs::Vector3 WindowsDetector::normalizedVectorFrom2Points(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2)
{
    geometry_msgs::Vector3 vector;
    vector.x = point_2.x - point_1.x;
    vector.y = point_2.y - point_1.y;
    vector.z = point_2.z - point_1.z;

    double vector_norm = sqrt(pow(vector.x,2) + pow(vector.y,2) + pow(vector.z,2));
    vector.x = vector.x/vector_norm;
    vector.y = vector.y/vector_norm;
    vector.z = vector.z/vector_norm;
    return vector;
}

geometry_msgs::Vector3 WindowsDetector::normalizedCrossProduct(geometry_msgs::Vector3& vector_1, geometry_msgs::Vector3& vector_2)
{
    geometry_msgs::Vector3 vector;
    vector.x = vector_1.y * vector_2.z - vector_1.z * vector_2.y;
    vector.y = - (vector_1.x * vector_2.z - vector_1.z * vector_2.x);
    vector.z = vector_1.x * vector_2.y - vector_1.y * vector_2.x;

    double vector_norm = sqrt(pow(vector.x,2) + pow(vector.y,2) + pow(vector.z,2));
    vector.x = vector.x/vector_norm;
    vector.y = vector.y/vector_norm;
    vector.z = vector.z/vector_norm;
    return vector;
}

geometry_msgs::Point WindowsDetector::calcWindowCenter(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2, geometry_msgs::Point& point_3, geometry_msgs::Point& point_4)
{
    geometry_msgs::Point center;

    center.x = (point_1.x + point_2.x + point_3.x + point_4.x)/4;
    center.y = (point_1.y + point_2.y + point_3.y + point_4.y)/4;
    center.z = (point_1.z + point_2.z + point_3.z + point_4.z)/4;

    return center;
}

float WindowsDetector::calcDistanceBetween3DPoints(geometry_msgs::Point& point_1, geometry_msgs::Point& point_2)
{
    return sqrt( (point_1.x-point_2.x)*(point_1.x-point_2.x) + (point_1.y-point_2.y)*(point_1.y-point_2.y) + (point_1.z-point_2.z) );
}

void WindowsDetector::get3DPointCameraModel(geometry_msgs::Point &point, float &depth, int pixel_row, int pixel_col)
{
    //std::cout << "Depth: " << depth << std::endl;
    //std::cout << "Pixel column: " << pixel_col << std::endl;
    //std::cout << "Pixel row: " << pixel_row << std::endl;
    point.x = (pixel_col - model_.cx())/model_.fx() * depth;
    point.y = (pixel_row - model_.cy())/model_.fy() * depth;
    point.z = depth;
}

void WindowsDetector::bubbleSort(std::vector<cv::Point2f>& vector_point_pair)
{
    int n = vector_point_pair.size();
    int i,j;
    bool swapped;

    // First order using x coordinate
    for (i=0; i < n-1; i++)
    {
        swapped = false;
        for (j=0; j < n-i-1; j++)
        {
            if (vector_point_pair[j].x > vector_point_pair[j+1].x)
            {
                swapPairs(vector_point_pair, j, j+1);
                swapped = true;
            }
        }
        if (swapped == false)
            break;
    }
    // Then order using y coordinate (half vector first, then the second half)
    for (i=0; i < n/2-1; i++)
    {
        swapped = false;
        for (j=0; j < n/2-i-1; j++)
        {
            if (vector_point_pair[j].y > vector_point_pair[j+1].y)
            {
                swapPairs(vector_point_pair, j, j+1);
                swapped = true;
            }
        }
        if (swapped == false)
            break;
    }
    for (i=0; i < n/2-1; i++)
    {
        swapped = false;
        for (j=n/2; j < n-i-1; j++)
        {
            if (vector_point_pair[j].y > vector_point_pair[j+1].y)
            {
                swapPairs(vector_point_pair, j, j+1);
                swapped = true;
            }
        }
        if (swapped == false)
            break;
    }
}

void WindowsDetector::swapPairs(std::vector<cv::Point2f>& vector_point_pair, int index_1, int index_2)
{
    cv::Point2f temp;
    temp = vector_point_pair.at(index_1);
    vector_point_pair.at(index_1) = vector_point_pair.at(index_2);
    vector_point_pair.at(index_2) = temp;
}

#define DEPTH_MAX_VALUE 6
inline uint8_t WindowsDetector::mapFloatDepthImageToUInt8(float input)
{
    return floor((input/DEPTH_MAX_VALUE)*255);
}

void WindowsDetector::depthCallback(const sensor_msgs::ImageConstPtr &image_ptr, const sensor_msgs::CameraInfoConstPtr &info_ptr)
{
    // For each 4 messages process 1 and discard the other 3
    if (image_counter_ == 0 || image_counter_ == 4)
    {
        image_counter_ = 0;
        model_.fromCameraInfo(info_ptr);

        try
        {
            //std::cout << "received image encoding " << image_ptr->encoding << std::endl;
            received_depth_image_opencv_ptr_ = cv_bridge::toCvShare(image_ptr, sensor_msgs::image_encodings::TYPE_32FC1);
            //std::cout << "opencv bridge image encoding " << received_depth_image_opencv_ptr_->encoding << std::endl;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }

        int image_height = image_ptr->height;
        int image_width = image_ptr->width;

        cv_bridge::CvImage depth_image_8, edges_image, hough_lines_image, hough_lines_p_image;
        depth_image_8.image = cv::Mat(cv::Size(image_width, image_height), CV_8UC1);
        depth_image_8.header.frame_id = image_ptr->header.frame_id;
        depth_image_8.header.stamp = image_ptr->header.stamp;

        edges_image.header.frame_id = image_ptr->header.frame_id;
        hough_lines_image.header.frame_id = image_ptr->header.frame_id;
        hough_lines_p_image.header.frame_id = image_ptr->header.frame_id;
        edges_image.header.stamp = image_ptr->header.stamp;
        hough_lines_image.header.stamp = image_ptr->header.stamp;
        hough_lines_p_image.header.stamp = image_ptr->header.stamp;
        edges_image.encoding = sensor_msgs::image_encodings::MONO8;
        hough_lines_image.encoding = sensor_msgs::image_encodings::BGR8;
        hough_lines_p_image.encoding = sensor_msgs::image_encodings::BGR8;

        for(int i1 = 0; i1 < received_depth_image_opencv_ptr_->image.rows; i1++)
        {
            for(int j1 = 0; j1 < received_depth_image_opencv_ptr_->image.cols; j1++)
            {
                const cv::Mat & original_image_ref = received_depth_image_opencv_ptr_->image;
                const cv::Mat & converted_image_ref = depth_image_8.image;
                float *  value_original_ptr = (float *) (original_image_ref.data + original_image_ref.step[0]*i1 + original_image_ref.step[1]*j1);
                uint8_t * value_converted_ptr = (uint8_t *) (converted_image_ref.data + converted_image_ref.step[0] * i1 + converted_image_ref.step[1] * j1);

                uint32_t integer_aux;
                std::memcpy(&integer_aux, value_original_ptr, sizeof(float));
                //std::cout << std::hex << integer_aux << std::dec << ",";
                if (integer_aux == 0X7FC00000)
                {
                    *value_converted_ptr = 0;
                }
                else
                {
                    *value_converted_ptr = this->mapFloatDepthImageToUInt8(*value_original_ptr);
                }
            }
        }
        std::cout << std::endl;

        depth_image_8_ = depth_image_8;

        cv::Canny(depth_image_8.image, edges_image.image, 50, 200, 3, true);
        cv::cvtColor(edges_image.image, hough_lines_image.image, CV_GRAY2BGR);
        cv::cvtColor(edges_image.image, hough_lines_p_image.image, CV_GRAY2BGR);

        std::vector<cv::Vec2f> lines;
        HoughLines(edges_image.image, lines, 1, CV_PI/180, 100, 0, 0);

        std::cout << "Hough Detected lines" << std::endl;
        for (size_t i=0; i < lines.size(); i++)
        {
            cv::Scalar color;
            switch (i%3)
            {
            case 0:
                color = cv::Scalar(0,0,255);
            case 1:
                color = cv::Scalar(0,255,0);
            case 2:
                color = cv::Scalar(255,0,0);
            default:
                break;
            }
            float rho = lines[i][0], theta = lines[i][1];
            cv::Point pt1, pt2;
            double a = cos(theta), b = sin(theta);
            double x0 = a*rho, y0 = b*rho;
            pt1.x = cvRound(x0 + 2000*(-b));
            pt1.y = cvRound(y0 + 2000*(a));
            pt2.x = cvRound(x0 - 2000*(-b));
            pt2.y = cvRound(y0 - 2000*(a));

            std::cout << "Line " << i << " from: (" << pt1.x << "," << pt1.y << ") to " << "(" << pt2.x << "," << pt2.y << ")" << std::endl;
            std::cout << "rho: " << rho << " theta: " << theta << std::endl;
            std::cout << "Line " << i << " equation: y = " << -1/tan(theta) << "x + " << rho/sin(theta) << std::endl;
            line( hough_lines_image.image, pt1, pt2, color, 3, CV_AA);
        }

        std::vector<cv::Vec4i> lines_p;
        HoughLinesP(edges_image.image, lines_p, 1, CV_PI/180, 50, 50, 100);
        std::cout << "Probabilistic Hough Detected lines" << std::endl;
        for (size_t i = 0; i < lines_p.size(); i++)
        {
           cv::Scalar color;
           switch (i%3)
           {
           case 0:
               color = cv::Scalar(0,0,255);
               break;
           case 1:
               color = cv::Scalar(0,255,0);
               break;
           case 2:
               color = cv::Scalar(255,0,0);
               break;
           default:
               break;
           }

           cv::Vec4i l_p = lines_p[i];
           line(hough_lines_p_image.image, cv::Point(l_p[0], l_p[1]), cv::Point(l_p[2], l_p[3]), color, 3, CV_AA);
           std::cout << "Line " << i << " from: (" << l_p[0] << "," << l_p[1] << ") to (" << l_p[2] << "," << l_p[3] << ")" << std::endl;
        }

        //cv::imshow(RECEIVED_IMAGE_WINDOW, received_depth_image_opencv_ptr_->image);
        cv::imshow(CONVERTED_DEPTH_IMAGE_WINDOW, depth_image_8.image);
        cv::imshow(EDGES_IMAGE_WINDOW, edges_image.image);
        cv::imshow(HOUGH_LINES_IMAGE_WINDOW, hough_lines_image.image);
        cv::imshow(HOUGH_LINES_P_IMAGE_WINDOW, hough_lines_p_image.image);
        //cv::imshow(CORNER_DETECTION_WINDOW, depth_image_8_.image);
        this->detectWindows();
        image_pub_.publish(hough_lines_image.toImageMsg());

        //cv::waitKey();
    }
    image_counter_++;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "windows_detector");
    WindowsDetector windows_detector;
    ros::Rate rate(20);
    while(ros::ok())
    {
        ros::spinOnce();
        cv::waitKey(1);
        rate.sleep();
    }

    return 0;
}


#include <sparse.h>
#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(col_prod_with_transpose)
{
  // setup test
  typedef sparse::Block<6,4,float> block6x4_type;
  typedef sparse::Block<4,6,float> block4x6_type;
  typedef sparse::Block<4,1,float> block4x1_type;
  typedef sparse::Block<6,1,float> block6x1_type;
  typedef sparse::CompressedRowMatrix<block6x4_type> matrix6x4_type;
  typedef sparse::CompressedRowMatrix<block4x6_type> matrix4x6_type;
  typedef sparse::Vector<block6x1_type> vector_type;
  typedef sparse::CompressedVector<block6x1_type> compressed_vector_type;
  typedef sparse::CompressedVector<block6x1_type, sparse::detail::FastVectorCompressor> fast_compressed_vector_type;

  matrix6x4_type JT(4,3,6);
  sparse::fill(JT(0,0), 0.0f);
  sparse::fill(JT(1,1), 0.0f);
  sparse::fill(JT(1,2), 0.0f);
  sparse::fill(JT(2,0), 0.0f);
  sparse::fill(JT(2,2), 0.0f);
  sparse::fill(JT(3,1), 0.0f);

  matrix4x6_type J;
  sparse::transpose(JT, J); // exploiting that J = J^T^T

  vector_type w(4);
  sparse::fill(w(0));
  sparse::fill(w(1));
  sparse::fill(w(2));
  sparse::fill(w(3));

  block6x1_type to_add;
  to_add[0] = 14;
  to_add[1] = 38;
  to_add[2] = 62;
  to_add[3] = 86;
  to_add[4] = 110;
  to_add[5] = 134;

  block6x1_type original;
  sparse::fill(original,0.0f);

  block6x1_type result;

  block4x1_type delta_x(0);
  sparse::fill(delta_x);

  // test column prod
  column_prod( JT, J, delta_x, w, 0);
  add(original, to_add, result);
  BOOST_CHECK( w(0) == result );
  BOOST_CHECK( w(1) == original );
  BOOST_CHECK( w(2) == result );
  BOOST_CHECK( w(3) == original);

  column_prod( JT, J, delta_x, w, 1);
  BOOST_CHECK( w(0) == result );
  BOOST_CHECK( w(1) == result );
  BOOST_CHECK( w(2) == result );
  BOOST_CHECK( w(3) == result );

  column_prod( JT, J, delta_x, w, 2);
  BOOST_CHECK( w(0) == result );
  BOOST_CHECK( w(3) == result );
  add(to_add, result);
  BOOST_CHECK( w(1) == result );
  BOOST_CHECK( w(2) == result );

  // initialize a dense compressed vector and test again
  compressed_vector_type c_w(4,4);
  sparse::fill(c_w(0));
  sparse::fill(c_w(1));
  sparse::fill(c_w(2));
  sparse::fill(c_w(3));

  column_prod( JT, J, delta_x, c_w, 0);
  add(original, to_add, result);
  BOOST_CHECK( c_w(0) == result );
  BOOST_CHECK( c_w(1) == original );
  BOOST_CHECK( c_w(2) == result );
  BOOST_CHECK( c_w(3) == original);

  column_prod( JT, J, delta_x, c_w, 1);
  BOOST_CHECK( c_w(0) == result );
  BOOST_CHECK( c_w(1) == result );
  BOOST_CHECK( c_w(2) == result );
  BOOST_CHECK( c_w(3) == result );

  column_prod( JT, J, delta_x, c_w, 2);
  BOOST_CHECK( c_w(0) == result );
  BOOST_CHECK( c_w(3) == result );
  add(to_add, result);
  BOOST_CHECK( c_w(1) == result );
  BOOST_CHECK( c_w(2) == result );

  // initialize a dense fast compressed vector and test again
  fast_compressed_vector_type f_w(4,4);
  sparse::fill(f_w(0));
  sparse::fill(f_w(1));
  sparse::fill(f_w(2));
  sparse::fill(f_w(3));

  column_prod( JT, J, delta_x, f_w, 0);
  add(original, to_add, result);
  BOOST_CHECK( f_w(0) == result );
  BOOST_CHECK( f_w(1) == original );
  BOOST_CHECK( f_w(2) == result );
  BOOST_CHECK( f_w(3) == original);

  column_prod( JT, J, delta_x, f_w, 1);
  BOOST_CHECK( f_w(0) == result );
  BOOST_CHECK( f_w(1) == result );
  BOOST_CHECK( f_w(2) == result );
  BOOST_CHECK( f_w(3) == result );

  column_prod( JT, J, delta_x, f_w, 2);
  BOOST_CHECK( f_w(0) == result );
  BOOST_CHECK( f_w(3) == result );
  add(to_add, result);
  BOOST_CHECK( f_w(1) == result );
  BOOST_CHECK( f_w(2) == result );
}
BOOST_AUTO_TEST_SUITE_END();


/****************************************************************************
 *  hipipe library
 *  Copyright (c) 2017, Cognexa Solutions s.r.o.
 *  Copyright (c) 2018, Iterait a.s.
 *  Author(s) Filip Matzner
 *
 *  This file is distributed under the MIT License.
 *  See the accompanying file LICENSE.txt for the complete license agreement.
 ****************************************************************************/

#include <boost/algorithm/string.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>

#include <hipipe/core/base64.hpp>

namespace hipipe {

std::vector<std::uint8_t> base64_decode(const std::string& b64data)
{
    namespace bai = boost::archive::iterators;
    using It =
      bai::transform_width<bai::binary_from_base64<std::string::const_iterator>, 8, 6>;
    // skip padding characters
    std::size_t len = b64data.size();
    while (len && b64data[len - 1] == '=') --len;
    return std::vector<std::uint8_t>(It(std::begin(b64data)), It(std::begin(b64data) + len));
}

std::string base64_encode(const std::vector<std::uint8_t>& data)
{
    namespace bai = boost::archive::iterators;
    using It =
      bai::base64_from_binary<bai::transform_width<std::vector<std::uint8_t>::const_iterator, 6, 8>>;
    std::string res(It(std::begin(data)), It(std::end(data)));
    return res.append((3 - data.size() % 3) % 3, '=');
}

} // end namespace hipipe


#include <iostream>

#include <geometry/object3d.hpp>
#include <Eigen/Dense>

using namespace std;
using namespace lsfm;

typedef double FT;

int main(int argc, char** argv)
{
    string f = "/media/lange/Data/Blender/example.obj";
    Object3DList<FT> objs;
    loadObjects(f,objs);
    Eigen::Array<double,Eigen::Dynamic,Eigen::Dynamic> test(3,2);
    test << 1.0 , 2.0 , 3.0 , 4.0 , 5.0 , 6.0;
    Eigen::Vector3d vec;
    cout << sizeof(vec) << " " << sizeof(vec) / 8 << endl;
    Eigen::Matrix<double,3,3> m;
    cout << sizeof(m) << " " << sizeof(m) / 8 << endl;

    for_each(objs.begin(),objs.end(),[](const Object3D<FT> obj) {
        std::cout << obj;
    });

    return 0;
}


/*
 *  Copyright (c) 2014, Facebook, Inc.
 *  All rights reserved.
 *
 *  This source code is licensed under the BSD-style license found in the
 *  LICENSE file in the root directory of this source tree. An additional grant
 *  of patent rights can be found in the PATENTS file in the same directory.
 *
 */

#include <sstream>

#include <boost/algorithm/string.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>

#include "osquery/core/conversions.h"

namespace bai = boost::archive::iterators;

namespace osquery {

typedef bai::binary_from_base64<const char*> base64_str;
typedef bai::transform_width<base64_str, 8, 6> base64_dec;
typedef bai::transform_width<std::string::const_iterator, 6, 8> base64_enc;
typedef bai::base64_from_binary<base64_enc> it_base64;

std::string base64Decode(const std::string& encoded) {
  std::string is;
  std::stringstream os;

  is = encoded;
  boost::replace_all(is, "\r\n", "");
  boost::replace_all(is, "\n", "");
  uint32_t size = is.size();

  // Remove the padding characters
  if (size && is[size - 1] == '=') {
    --size;
    if (size && is[size - 1] == '=') {
      --size;
    }
  }

  if (size == 0) {
    return "";
  }

  std::copy(base64_dec(is.data()),
            base64_dec(is.data() + size),
            std::ostream_iterator<char>(os));

  return os.str();
}

std::string base64Encode(const std::string& unencoded) {
  std::stringstream os;

  if (unencoded.size() == 0) {
    return std::string();
  }

  unsigned int writePaddChars = (3-unencoded.length()%3)%3;
  std::string base64(it_base64(unencoded.begin()), it_base64(unencoded.end()));
  base64.append(writePaddChars,'=');
  os << base64;
  return os.str();
}

bool isPrintable(const std::string& check) {
  for (const unsigned char ch : check) {
    if (ch >= 0x7F || ch <= 0x1F) {
      return false;
    }
  }
  return true;
}
}


//  ratio.hpp  ---------------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*

This code was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.4 Compile-time rational arithmetic [ratio], of the C++ committee working
paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

// The way overflow is managed for ratio_less is taken from
// llvm/libcxx/include/ratio

#ifndef BOOST_RATIO_DETAIL_RATIO_OPERATIONS_HPP
#define BOOST_RATIO_DETAIL_RATIO_OPERATIONS_HPP

#include <boost/core/enable_if.hpp>
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>
#include <boost/ratio/config.hpp>
#include <boost/ratio/detail/mpl/abs.hpp>
#include <boost/ratio/detail/mpl/sign.hpp>
#include <boost/type_traits/integral_constant.hpp>
#include <climits>
#include <cstdlib>
#include <limits>

//
// We simply cannot include this header on gcc without getting copious warnings
// of the kind:
//
// boost/integer.hpp:77:30: warning: use of C99 long long integer constant
//
// And yet there is no other reasonable implementation, so we declare this a
// system header to suppress these warnings.
//
#if defined(__GNUC__) && (__GNUC__ >= 4)
#pragma GCC system_header
#endif

namespace boost
{

//----------------------------------------------------------------------------//
//                                 helpers                                    //
//----------------------------------------------------------------------------//

namespace ratio_detail
{

template <boost::intmax_t X, boost::intmax_t Y,
          boost::intmax_t = mpl::sign_c<boost::intmax_t, Y>::value>
class br_add;

template <boost::intmax_t X, boost::intmax_t Y>
class br_add<X, Y, 1>
{
    static const boost::intmax_t min =
        boost::integer_traits<boost::intmax_t>::const_min;
    static const boost::intmax_t max =
        boost::integer_traits<boost::intmax_t>::const_max;

    BOOST_RATIO_STATIC_ASSERT(X <= max - Y, BOOST_RATIO_OVERFLOW_IN_ADD, ());

  public:
    static const boost::intmax_t value = X + Y;
};

template <boost::intmax_t X, boost::intmax_t Y>
class br_add<X, Y, 0>
{
  public:
    static const boost::intmax_t value = X;
};

template <boost::intmax_t X, boost::intmax_t Y>
class br_add<X, Y, -1>
{
    static const boost::intmax_t min =
        boost::integer_traits<boost::intmax_t>::const_min;
    static const boost::intmax_t max =
        boost::integer_traits<boost::intmax_t>::const_max;

    BOOST_RATIO_STATIC_ASSERT(min - Y <= X, BOOST_RATIO_OVERFLOW_IN_ADD, ());

  public:
    static const boost::intmax_t value = X + Y;
};

template <boost::intmax_t X, boost::intmax_t Y,
          boost::intmax_t = mpl::sign_c<boost::intmax_t, Y>::value>
class br_sub;

template <boost::intmax_t X, boost::intmax_t Y>
class br_sub<X, Y, 1>
{
    static const boost::intmax_t min =
        boost::integer_traits<boost::intmax_t>::const_min;
    static const boost::intmax_t max =
        boost::integer_traits<boost::intmax_t>::const_max;

    BOOST_RATIO_STATIC_ASSERT(min + Y <= X, BOOST_RATIO_OVERFLOW_IN_SUB, ());

  public:
    static const boost::intmax_t value = X - Y;
};

template <boost::intmax_t X, boost::intmax_t Y>
class br_sub<X, Y, 0>
{
  public:
    static const boost::intmax_t value = X;
};

template <boost::intmax_t X, boost::intmax_t Y>
class br_sub<X, Y, -1>
{
    static const boost::intmax_t min =
        boost::integer_traits<boost::intmax_t>::const_min;
    static const boost::intmax_t max =
        boost::integer_traits<boost::intmax_t>::const_max;

    BOOST_RATIO_STATIC_ASSERT(X <= max + Y, BOOST_RATIO_OVERFLOW_IN_SUB, ());

  public:
    static const boost::intmax_t value = X - Y;
};

template <boost::intmax_t X, boost::intmax_t Y>
class br_mul
{
    static const boost::intmax_t nan = boost::intmax_t(
        BOOST_RATIO_UINTMAX_C(1) << (sizeof(boost::intmax_t) * CHAR_BIT - 1));
    static const boost::intmax_t min =
        boost::integer_traits<boost::intmax_t>::const_min;
    static const boost::intmax_t max =
        boost::integer_traits<boost::intmax_t>::const_max;

    static const boost::intmax_t a_x = mpl::abs_c<boost::intmax_t, X>::value;
    static const boost::intmax_t a_y = mpl::abs_c<boost::intmax_t, Y>::value;

    BOOST_RATIO_STATIC_ASSERT(X != nan, BOOST_RATIO_OVERFLOW_IN_MUL, ());
    BOOST_RATIO_STATIC_ASSERT(Y != nan, BOOST_RATIO_OVERFLOW_IN_MUL, ());
    BOOST_RATIO_STATIC_ASSERT(a_x <= max / a_y, BOOST_RATIO_OVERFLOW_IN_MUL,
                              ());

  public:
    static const boost::intmax_t value = X * Y;
};

template <boost::intmax_t Y>
class br_mul<0, Y>
{
  public:
    static const boost::intmax_t value = 0;
};

template <boost::intmax_t X>
class br_mul<X, 0>
{
  public:
    static const boost::intmax_t value = 0;
};

template <>
class br_mul<0, 0>
{
  public:
    static const boost::intmax_t value = 0;
};

// Not actually used but left here in case needed in future maintenance
template <boost::intmax_t X, boost::intmax_t Y>
class br_div
{
    static const boost::intmax_t nan = boost::intmax_t(
        BOOST_RATIO_UINTMAX_C(1) << (sizeof(boost::intmax_t) * CHAR_BIT - 1));
    static const boost::intmax_t min =
        boost::integer_traits<boost::intmax_t>::const_min;
    static const boost::intmax_t max =
        boost::integer_traits<boost::intmax_t>::const_max;

    BOOST_RATIO_STATIC_ASSERT(X != nan, BOOST_RATIO_OVERFLOW_IN_DIV, ());
    BOOST_RATIO_STATIC_ASSERT(Y != nan, BOOST_RATIO_OVERFLOW_IN_DIV, ());
    BOOST_RATIO_STATIC_ASSERT(Y != 0, BOOST_RATIO_DIVIDE_BY_0, ());

  public:
    static const boost::intmax_t value = X / Y;
};

// ratio arithmetic
template <class R1, class R2>
struct ratio_add;
template <class R1, class R2>
struct ratio_subtract;
template <class R1, class R2>
struct ratio_multiply;
template <class R1, class R2>
struct ratio_divide;

template <class R1, class R2>
struct ratio_add
{
    // The nested typedef type shall be a synonym for ratio<T1, T2>::type where
    // T1 has the value R1::num * R2::den + R2::num * R1::den and T2 has the
    // value R1::den * R2::den.
    // As the preceding doesn't works because of overflow on boost::intmax_t we
    // need something more elaborated.
  private:
    static const boost::intmax_t gcd_n1_n2 =
        mpl::gcd_c<boost::intmax_t, R1::num, R2::num>::value;
    static const boost::intmax_t gcd_d1_d2 =
        mpl::gcd_c<boost::intmax_t, R1::den, R2::den>::value;

  public:
    // No need to normalize as ratio_multiply is already normalized
    typedef typename ratio_multiply<
        ratio<gcd_n1_n2, R1::den / gcd_d1_d2>,
        ratio<boost::ratio_detail::br_add<
                  boost::ratio_detail::br_mul<R1::num / gcd_n1_n2,
                                              R2::den / gcd_d1_d2>::value,
                  boost::ratio_detail::br_mul<
                      R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value>::value,
              R2::den>>::type type;
};
template <class R, boost::intmax_t D>
struct ratio_add<R, ratio<0, D>>
{
    typedef R type;
};

template <class R1, class R2>
struct ratio_subtract
{
    // The nested typedef type shall be a synonym for ratio<T1, T2>::type where
    // T1 has the value
    // R1::num *R2::den - R2::num * R1::den and T2 has the value R1::den *
    // R2::den. As the preceding doesn't works because of overflow on
    // boost::intmax_t we need something more elaborated.
  private:
    static const boost::intmax_t gcd_n1_n2 =
        mpl::gcd_c<boost::intmax_t, R1::num, R2::num>::value;
    static const boost::intmax_t gcd_d1_d2 =
        mpl::gcd_c<boost::intmax_t, R1::den, R2::den>::value;

  public:
    // No need to normalize as ratio_multiply is already normalized
    typedef typename ratio_multiply<
        ratio<gcd_n1_n2, R1::den / gcd_d1_d2>,
        ratio<boost::ratio_detail::br_sub<
                  boost::ratio_detail::br_mul<R1::num / gcd_n1_n2,
                                              R2::den / gcd_d1_d2>::value,
                  boost::ratio_detail::br_mul<
                      R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value>::value,
              R2::den>>::type type;
};

template <class R, boost::intmax_t D>
struct ratio_subtract<R, ratio<0, D>>
{
    typedef R type;
};

template <class R1, class R2>
struct ratio_multiply
{
    // The nested typedef type  shall be a synonym for ratio<R1::num * R2::den -
    // R2::num * R1::den, R1::den * R2::den>::type. As the preceding doesn't
    // works because of overflow on boost::intmax_t we need something more
    // elaborated.
  private:
    static const boost::intmax_t gcd_n1_d2 =
        mpl::gcd_c<boost::intmax_t, R1::num, R2::den>::value;
    static const boost::intmax_t gcd_d1_n2 =
        mpl::gcd_c<boost::intmax_t, R1::den, R2::num>::value;

  public:
    typedef typename ratio<
        boost::ratio_detail::br_mul<R1::num / gcd_n1_d2,
                                    R2::num / gcd_d1_n2>::value,
        boost::ratio_detail::br_mul<R2::den / gcd_n1_d2,
                                    R1::den / gcd_d1_n2>::value>::type type;
};

template <class R1, class R2>
struct ratio_divide
{
    // The nested typedef type  shall be a synonym for ratio<R1::num * R2::den,
    // R2::num * R1::den>::type. As the preceding doesn't works because of
    // overflow on boost::intmax_t we need something more elaborated.
  private:
    static const boost::intmax_t gcd_n1_n2 =
        mpl::gcd_c<boost::intmax_t, R1::num, R2::num>::value;
    static const boost::intmax_t gcd_d1_d2 =
        mpl::gcd_c<boost::intmax_t, R1::den, R2::den>::value;

  public:
    typedef typename ratio<
        boost::ratio_detail::br_mul<R1::num / gcd_n1_n2,
                                    R2::den / gcd_d1_d2>::value,
        boost::ratio_detail::br_mul<R2::num / gcd_n1_n2,
                                    R1::den / gcd_d1_d2>::value>::type type;
};
template <class R1, class R2>
struct is_evenly_divisible_by
{
  private:
    static const boost::intmax_t gcd_n1_n2 =
        mpl::gcd_c<boost::intmax_t, R1::num, R2::num>::value;
    static const boost::intmax_t gcd_d1_d2 =
        mpl::gcd_c<boost::intmax_t, R1::den, R2::den>::value;

  public:
    typedef integral_constant<bool, ((R2::num / gcd_n1_n2 == 1) &&
                                     (R1::den / gcd_d1_d2) == 1)>
        type;
};

template <class T>
struct is_ratio : public boost::false_type
{
};
template <boost::intmax_t N, boost::intmax_t D>
struct is_ratio<ratio<N, D>> : public boost::true_type
{
};

template <class R1, class R2, boost::intmax_t Q1 = R1::num / R1::den,
          boost::intmax_t M1 = R1::num % R1::den,
          boost::intmax_t Q2 = R2::num / R2::den,
          boost::intmax_t M2 = R2::num % R2::den>
struct ratio_less1
{
    static const bool value = Q1 < Q2;
};

template <class R1, class R2, boost::intmax_t Q>
struct ratio_less1<R1, R2, Q, 0, Q, 0>
{
    static const bool value = false;
};

template <class R1, class R2, boost::intmax_t Q, boost::intmax_t M2>
struct ratio_less1<R1, R2, Q, 0, Q, M2>
{
    static const bool value = true;
};

template <class R1, class R2, boost::intmax_t Q, boost::intmax_t M1>
struct ratio_less1<R1, R2, Q, M1, Q, 0>
{
    static const bool value = false;
};

template <class R1, class R2, boost::intmax_t Q, boost::intmax_t M1,
          boost::intmax_t M2>
struct ratio_less1<R1, R2, Q, M1, Q, M2>
{
    static const bool value =
        ratio_less1<ratio<R2::den, M2>, ratio<R1::den, M1>>::value;
};

template <class R1, class R2,
          boost::intmax_t S1 = mpl::sign_c<boost::intmax_t, R1::num>::value,
          boost::intmax_t S2 = mpl::sign_c<boost::intmax_t, R2::num>::value>
struct ratio_less
{
    static const bool value = S1 < S2;
};

template <class R1, class R2>
struct ratio_less<R1, R2, 1LL, 1LL>
{
    static const bool value = ratio_less1<R1, R2>::value;
};

template <class R1, class R2>
struct ratio_less<R1, R2, -1LL, -1LL>
{
    static const bool value =
        ratio_less1<ratio<-R2::num, R2::den>, ratio<-R1::num, R1::den>>::value;
};

} // namespace ratio_detail

} // namespace boost

#endif // BOOST_RATIO_HPP


//==================================================================================================
/**
  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_COMMON_SIMD_FUNCTION_ALL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ALL_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/hmsb.hpp>
#include <boost/simd/function/splatted.hpp>

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

  BOOST_DISPATCH_OVERLOAD_IF( all_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::fundamental_<A0>, X>
                            )
  {
    // MSVC has some issue with >> in template declaration
    static const std::size_t cnt = (std::size_t(-1) >> (64 - A0::static_size));
    using count = std::integral_constant<std::size_t,cnt>;

    BOOST_FORCEINLINE bool operator()(const A0& a0) const BOOST_NOEXCEPT
    {
      return hmsb(genmask(a0)) == count::value;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF( all_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::splatted_tag
                            , bs::pack_<bd::fundamental_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE as_logical_t<A0>
    operator()(bs::splatted_tag const&, const A0& a0) const BOOST_NOEXCEPT
    {
      return as_logical_t<A0>(all(a0));
    }
  };
} } }

#endif


// The core of this code is based on Kevin Cowtan's
// clipper/examples/csigmaa.cpp

#include <cctbx/boost_python/flex_fwd.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

#if defined(__linux__) && defined(__GNUC__) \
 && __GNUC__ == 2 && __GNUC_MINOR__ == 96
# undef isnan
#endif

#include <clipper/clipper.h>
#include <clipper/clipper-contrib.h>
#include <clipper/cctbx/clipper_cctbx.h>

namespace clipper { namespace boost_python {

  class SFweight_spline_interface
  {
    public:
      SFweight_spline_interface(
        cctbx::uctbx::unit_cell const& unit_cell,
        cctbx::sgtbx::space_group const& space_group,
        af::shared<cctbx::miller::index<> > const& miller_indices,
        bool anomalous_flag,
        af::const_ref<double> const& f_obs_data,
        af::const_ref<double> const& f_obs_sigmas,
        af::const_ref<std::complex<double> > const& f_calc,
        af::const_ref<bool> const& test_set_flags,
        int n_refln=1000,
        int n_param=20)
      :
        miller_indices_(miller_indices),
        hkls_(CCTBX::as_HKL_info(
          unit_cell, space_group, miller_indices.const_ref())),
        fb_(hkls_),
        fd_(hkls_),
        phiw_(hkls_),
        sfweight_spline_(n_refln, n_param)
      {
        if (anomalous_flag) {
          throw cctbx::error(
            "SFweight_spline_interface cannot currently"
            " handle anomalous arrays.");
        }
        CCTBX_ASSERT(test_set_flags.size() == miller_indices.size());
        CCTBX_ASSERT(hkls_.num_reflections() == miller_indices.size());
        af::const_ref<cctbx::miller::index<> >
          mi_ref = miller_indices_.const_ref();
        HKL_data<data64::F_sigF> f_obs_ = CCTBX::as_HKL_data(
          hkls_, mi_ref, f_obs_data, f_obs_sigmas);
        HKL_data<data64::F_phi> f_calc_ = CCTBX::as_HKL_data(
          hkls_, mi_ref, f_calc);
        HKL_data<data64::Flag> flag(hkls_);
        for(std::size_t i=0;i<mi_ref.size();i++) {
          data64::Flag fl;
          for(std::size_t i=0;i<mi_ref.size();i++) {
            HKL h = CCTBX::Hkl(mi_ref[i]);
            fl.flag() = test_set_flags[i] ? SFweight_spline<double>::BOTH
                                          : SFweight_spline<double>::NONE;
            flag.set_data(h, fl);
          }
        }
        // do sigmaa calc
        CCTBX_ASSERT(sfweight_spline_(fb_, fd_, phiw_, f_obs_, f_calc_, flag));
      }

      std::size_t
      number_of_spline_parameters()
      {
        return sfweight_spline_.params_scale().size();
      }

      af::shared<std::complex<double> >
      fb() const
      {
        return CCTBX::extract_complex(fb_, miller_indices_.const_ref());
      }

      af::shared<std::complex<double> >
      fd() const
      {
        return CCTBX::extract_complex(fd_, miller_indices_.const_ref());
      }

      af::shared<double>
      centroid_phases() const
      {
        return CCTBX::extract_centroid_phases(
          phiw_, miller_indices_.const_ref());
      }

      af::shared<double>
      figures_of_merit() const
      {
        return CCTBX::extract_figures_of_merit(
          phiw_, miller_indices_.const_ref());
      }

    protected:
      af::shared<cctbx::miller::index<> > miller_indices_;
      HKL_info hkls_;
      HKL_data<data64::F_phi> fb_;
      HKL_data<data64::F_phi> fd_;
      HKL_data<data64::Phi_fom> phiw_;
      SFweight_spline<double> sfweight_spline_;
  };

  struct SFweight_spline_interface_wrappers
  {
    typedef SFweight_spline_interface w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("SFweight_spline_interface", no_init)
        .def(init<cctbx::uctbx::unit_cell const&,
                  cctbx::sgtbx::space_group const&,
                  af::shared<cctbx::miller::index<> > const&,
                  bool,
                  af::const_ref<double> const&,
                  af::const_ref<double> const&,
                  af::const_ref<std::complex<double> > const&,
                  af::const_ref<bool> const&,
                  optional<int, int> >((
          arg("unit_cell"),
          arg("space_group"),
          arg("miller_indices"),
          arg("anomalous_flag"),
          arg("f_obs_data"),
          arg("f_obs_sigmas"),
          arg("f_calc"),
          arg("test_set_flags"),
          arg("n_refln")=1000,
          arg("n_param")=20)))
        .def("number_of_spline_parameters", &w_t::number_of_spline_parameters)
        .def("fb", &w_t::fb)
        .def("fd", &w_t::fd)
        .def("centroid_phases", &w_t::centroid_phases)
        .def("figures_of_merit", &w_t::figures_of_merit)
      ;
    }
  };

  void wrap_sigmaa()
  {
    SFweight_spline_interface_wrappers::wrap();
  }

}} // namespace clipper::boost_python


#include <cmath>
#include <map>

#include <ur10/ur10_inverse_kinematic.h>

#include <kdl/chain.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>

#include <xpp_states/cartesian_declarations.h>
#include <cstdlib>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#include <boost/date_time.hpp>
#include <trac_ik/trac_ik.hpp>
using namespace xpp;
using namespace KDL;

UR10InverseKinematics::VectorXd
UR10InverseKinematics::GetJointAngles(const Vector3d& ee_pos_B,const Eigen::Quaterniond& base_ori_B, const std::string& joint_desired_topic) const
{

const std::string joint_desired_topic_ = joint_desired_topic;

//load robot model
robot_model_loader::RobotModelLoader robot_model_loader("ur10_rviz_urdf_robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
ROS_DEBUG("Model frame: %s", kinematic_model->getModelFrame().c_str());
robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const robot_state::JointModelGroup* joint_model_group =  kinematic_model->getJointModelGroup("manipulator");
ROS_DEBUG("after getjointkinematics");
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
std::vector<double> joint_values;

//create joint group
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
for (std::size_t i = 0; i < joint_names.size(); ++i)
{
  ROS_DEBUG("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
}
kinematic_state->enforceBounds();
double time_out =0.01;

//set EE pose !!!!!!TODO!!!!!!!!
//end_effector_state.translation()=Eigen::Vector3d(0,0,0.5);
//end_effector_state.matrix()<<0,0,-1,ee_pos_B.x(),
//                            0,1,0,ee_pos_B.y(),
//                            1,0,0,ee_pos_B.z(),
//                            0,0,0,1;

//affin transformation matrxi from base frame to end effector's frame
Eigen::Isometry3d end_effector_state;
if(joint_desired_topic_ == "/xpp/joint_ur10_des_1"){

//  Eigen::Matrix3d initial_rotation;
//                  initial_rotation<<1,0,0,
//                                    0,-1,0,
//                                    0,0,1;
//  Eigen::Quaterniond initial_r_q(initial_rotation);
//  end_effector_state.rotate(initial_r_q);

//  end_effector_state.pretranslate(ee_pos_B);

  Eigen::Matrix4d base_ori_matrix = Eigen::Matrix4d::Identity();
  Eigen::Matrix4d initial_matrix;
  initial_matrix<<1,0,0,ee_pos_B.x(),
                  0,1,0,ee_pos_B.y(),
                  0,0,1,ee_pos_B.z(),
                  0,0,0,1;

  // ensure that the rotation angle is between -90 and 90 degree.
  Vector3d base_ori_euler = base_ori_B.toRotationMatrix().eulerAngles(2,1,0);
  for(int i=0;i<3;i++){
    while(base_ori_euler[i]>1.5707 || base_ori_euler[i]<-1.5707){
      if(base_ori_euler[i]>1.5707)
        base_ori_euler[i]-=1.5707;
      if(base_ori_euler[i]<-1.5707)
        base_ori_euler[i]+=1.5707;
    }
  }
  //convert euler angles to rotation matrix
  Eigen::Matrix3d m ;
  m=Eigen::AngleAxisd(base_ori_euler.x(),Eigen::Vector3d::UnitZ())
    *Eigen::AngleAxisd(base_ori_euler.y(), Eigen::Vector3d::UnitY())
    *Eigen::AngleAxisd(base_ori_euler.z(), Eigen::Vector3d::UnitX());
  base_ori_matrix.block<3,3>(0,0) = m;

  //base_ori_matrix.block<3,3>(0,0)= base_ori_B.toRotationMatrix();

  //initial rotation + base rotation
  end_effector_state.matrix()= initial_matrix*base_ori_matrix;
  //form: cubic, end effector rotation range (-90, 90)

  //std::cout<<"Transformation matrix is : "<<end_effector_state.rotation()<<std::endl;

}
if(joint_desired_topic_ == "/xpp/joint_ur10_des_2"){

//  Eigen::Matrix3d initial_rotation;
//                  initial_rotation<<-1,0,0,
//                                    0,-1,0,
//                                    0,0,1;
//  Eigen::Quaterniond initial_r_q(initial_rotation);

// // end_effector_state.rotate(Eigen::Quaterniond())= initial_r_q;
//  end_effector_state.pretranslate(ee_pos_B);

  Eigen::Matrix4d base_ori_matrix = Eigen::Matrix4d::Identity();
  Eigen::Matrix4d initial_matrix;
  initial_matrix<<-1,0,0,ee_pos_B.x(),
                  0,-1,0,ee_pos_B.y(),
                  0,0,1,ee_pos_B.z(),
                  0,0,0,1;

  Vector3d base_ori_euler = base_ori_B.toRotationMatrix().eulerAngles(2,1,0);
  for(int i=0;i<3;i++){
    while(base_ori_euler[i]>1.5707 || base_ori_euler[i]<-1.5707){
      if(base_ori_euler[i]>1.5707)
        base_ori_euler[i]-=1.5707;
      if(base_ori_euler[i]<-1.5707)
        base_ori_euler[i]+=1.5707;
    }
  }
  //convert euler angles to rotation matrix
  Eigen::Matrix3d m ;
  m=Eigen::AngleAxisd(base_ori_euler.x(),Eigen::Vector3d::UnitZ())
    *Eigen::AngleAxisd(base_ori_euler.y(), Eigen::Vector3d::UnitY())
    *Eigen::AngleAxisd(base_ori_euler.z(), Eigen::Vector3d::UnitX());
  base_ori_matrix.block<3,3>(0,0) = m;
  end_effector_state.matrix()= initial_matrix*base_ori_matrix;

//  end_effector_state.matrix()<<-1,0,0,ee_pos_B.x(),
//                                0,-1,0,ee_pos_B.y(),
//                                0,0,1,ee_pos_B.z(),
//                                0,0,0,1;
}
//std::cout<<end_effector_state.affine()<<std::endl;
//std::cout<<end_effector_state.matrix()<<std::endl;

//try 10 different initializations to solve the IK
//for(int i=0; i<10;i++){
  bool found_ik = kinematic_state->setFromIK(joint_model_group,end_effector_state,time_out);

  if (found_ik)
  {
    kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    for(std::size_t i=0; i < joint_names.size(); ++i)
    {
      ROS_DEBUG("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
    }
//    break;
  }
  else
  {
    ROS_INFO("Did not find IK solution");
  }
//}
//KDL::Chain chain;
//KDL::JntArray ll, ul;
//chain.addSegment(Segment("shoulder_pan_joint",Joint(Joint::RotZ),Frame(Rotation::RPY(0.0,0.0,0.0),Vector(0.0,0.0,0.1273))));
//chain.addSegment(Segment("shoulder_lift_joint",Joint(Joint::RotY),Frame(Rotation::RPY(0.0,1.570796325,0.0),Vector(0.0,0.220941,0.0))));
//chain.addSegment(Segment("elbow_joint",Joint(Joint::RotY),Frame(Rotation::RPY(0.0,0.0,0.0),Vector(0.0,-0.1719,0.612))));
//chain.addSegment(Segment("wrist_1_joint",Joint(Joint::RotY),Frame(Rotation::RPY(0.0,1.570796325,0.0),Vector(0.0,0.0,0.5723))));
//chain.addSegment(Segment("wrist_2_joint",Joint(Joint::RotZ),Frame(Rotation::RPY(0.0,0.0,0.0),Vector(0.0,0.1149,0.0))));
//chain.addSegment(Segment("wrist_3_joint",Joint(Joint::RotY),Frame(Rotation::RPY(0.0,0.0,0.0),Vector(0.0,0.0,0.1157))));
//chain.addSegment(Segment("ee_fixed_joint",Joint(Joint::None),Frame(Rotation::RPY(0.0,0.0,1.570796325),Vector(0.0,0.0922,0.0))));

////Creation of the solvers:
//ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
//ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
//ChainIkSolverPos_NR_JL iksolver1(chain,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6

////Creation of jntarrays:
//JntArray q(chain.getNrOfJoints());
//JntArray q_init(chain.getNrOfJoints());
////RRT
////for(int i=0;i<10;i++){
//  double j1= GetRandomNumber(-3.1415,3.1415);
//  double j2= GetRandomNumber(-3.1415,3.1415);
//  double j3= GetRandomNumber(-3.1415,3.1415);
//  double j4= GetRandomNumber(-3.1415,3.1415);
//  double j5= GetRandomNumber(-3.1415,3.1415);
//  double j6= GetRandomNumber(-3.1415,3.1415);

//  q_init.data<<0,j2,j3,0,j5,j6;
////  std::cout<<"q_init data: "<<q_init.data<<std::endl;
////  //Set destination frame
//  Frame F_dest;
//  F_dest.p = Vector(ee_pos_B.x(),ee_pos_B.y(),ee_pos_B.z());

//   F_dest.M = Rotation::RPY(0,-1.570796325,0);
//  int ret = iksolver1.CartToJnt(q_init,F_dest,q);
////  if(ret==0)
////    break;
////  else
////    std::cout<<"did not find the answer"<<std::endl;
//  std::cout<< "KDL result: "<< ret<<std::endl;

//}
//std::cout<< "KDL result number:"<<q.data<<std::endl;
Eigen::VectorXd b = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(joint_values.data(), joint_values.size());
return b;//q.data;

}

double
UR10InverseKinematics::GetRandomNumber(double fmin, double fmax) const{
  double f=(double)rand() / RAND_MAX;
  return fmin+f*(fmax-fmin);
}


#include <iostream>
#include <csignal>
#include <string>
#include <stdexcept>

#include <boost/scoped_ptr.hpp>
#include <boost/program_options.hpp>

#include <eigen3/Eigen/Core>

#include "FusionCore.h"
#include "FusionCoreConstants.h"
#include "FusionCoreConfig.h"
#include "../Utilities/MatrixConfig.h"

namespace po = boost::program_options;
using Eigen::Matrix;

boost::scoped_ptr<FusionCore> _fusionCore;

void signalHandler(int) {
    _fusionCore->stop();
}

int main(int argc, char** argv) {
    try {
        std::string configFile;
        std::string generatePath;

        po::options_description generalOptions("Options");
        generalOptions.add_options()
            ("help,h",   "print help message")
            ("config",   po::value<std::string>(&configFile),   "the configuration file to use")
            ("generate", po::value<std::string>(&generatePath), "generate default config files")
            ;

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).
                  options(generalOptions).
                  run(),
                  vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << generalOptions;
            return 0;
        }

        FusionCoreSettings settings;
        MatrixSettings<ANGULAR_STATE_DIM,       1>                       initialStateSettings;
        MatrixSettings<ANGULAR_STATE_DIM,       ANGULAR_STATE_DIM>       initialCovarianceSettings;
        MatrixSettings<ANGULAR_STATE_DIM,       ANGULAR_STATE_DIM>       processNoiseSettings;
        MatrixSettings<ANGULAR_MEASUREMENT_DIM, ANGULAR_MEASUREMENT_DIM> measurementNoiseSettings;

        if (vm.count("config")) {
            settings.load(configFile);
        } else {
            settings.serverID = SERVER_ID;
            settings.clientID = CLIENT_ID;
            settings.initialStatePath      = ANGULAR_INITIAL_STATE;
            settings.initialCovariancePath = ANGULAR_INITIAL_COVARIANCE;
            settings.processNoisePath      = ANGULAR_PROCESS_NOISE;
            settings.measurementNoisePath  = ANGULAR_MEASUREMENT_NOISE;
            if (vm.count("generate")) {
                settings.save(generatePath+"/FusionCore.json");

                initialStateSettings     .name = "initialstate";
                initialCovarianceSettings.name = "initialcovariance";
                processNoiseSettings     .name = "processnoise";
                measurementNoiseSettings .name = "measurementnoise";

                initialStateSettings     .save(generatePath+"/Angular/initialstate.json");
                initialCovarianceSettings.save(generatePath+"/Angular/initialcovariance.json");
                processNoiseSettings     .save(generatePath+"/Angular/processnoise.json");
                measurementNoiseSettings .save(generatePath+"/Angular/measurementnoise.json");

                return 0;
            }
        }

        if (settings.serverID < 0 || settings.serverID > 255) {
            throw std::runtime_error("Invalid server ID");
        }

        if (settings.clientID < 0 || settings.clientID > 255) {
            throw std::runtime_error("Invalid client ID");
        }

        initialStateSettings.load(settings.initialStatePath);
        Matrix<double, ANGULAR_STATE_DIM, 1> initialState;
        for (int i = 0; i < ANGULAR_STATE_DIM; i++) {
            initialState(i,0) = initialStateSettings.values[i][0];
        }

        initialCovarianceSettings.load(settings.initialCovariancePath);
        Matrix<double, ANGULAR_STATE_DIM, ANGULAR_STATE_DIM> initialCovariance;
        for (int i = 0; i < ANGULAR_STATE_DIM; i++) {
            for (int j = 0; j < ANGULAR_STATE_DIM; j++) {
                initialCovariance(i,j) = initialCovarianceSettings.values[i][j];
            }
        }

        processNoiseSettings.load(settings.processNoisePath);
        Matrix<double, ANGULAR_STATE_DIM, ANGULAR_STATE_DIM> processNoise;
        for (int i = 0; i < ANGULAR_STATE_DIM; i++) {
            for (int j = 0; j < ANGULAR_STATE_DIM; j++) {
                processNoise(i,j) = processNoiseSettings.values[i][j];
            }
        }

        measurementNoiseSettings.load(settings.measurementNoisePath);
        Matrix<double, ANGULAR_MEASUREMENT_DIM, ANGULAR_MEASUREMENT_DIM> measurementNoise;
        for (int i = 0; i < ANGULAR_MEASUREMENT_DIM; i++) {
            for (int j = 0; j < ANGULAR_MEASUREMENT_DIM; j++) {
                measurementNoise(i,j) = measurementNoiseSettings.values[i][j];
            }
        }

        _fusionCore.reset(new FusionCore(settings.serverID,
                                         settings.clientID,
                                         initialState,
                                         initialCovariance,
                                         processNoise,
                                         measurementNoise,
                                         3 - ANGULAR_STATE_DIM, //usually set to 3 - state size
                                         0.001,                 //usually set to a small positive value
                                         2));                   //2 is optimal for gaussian distributions
        std::signal(SIGINT, signalHandler);
        _fusionCore->start();
        return 0;
    } catch (std::exception& e) {
        std::cout << "Error: ";
        std::cout << e.what() << std::endl;
        return 1;
    }
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_SCALAR_REALLOG_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_REALLOG_HPP_INCLUDED

#include <boost/simd/function/definition/reallog.hpp>
#include <boost/simd/arch/common/scalar/function/reallog.hpp>

#endif