// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR

#include <boost/range.hpp>

#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/detail/disjoint/box_box.hpp>
#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_turn_info.hpp>
#include <boost/geometry/strategies/buffer.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

struct turn_get_box
{
    template <typename Box, typename Turn>
    static inline void apply(Box& total, Turn const& turn)
    {
        geometry::expand(total, turn.robust_point);
    }
};

struct turn_ovelaps_box
{
    template <typename Box, typename Turn>
    static inline bool apply(Box const& box, Turn const& turn)
    {
        return ! geometry::disjoint(box, turn.robust_point);
    }
};

template <typename Turns, typename Pieces>
class turn_in_piece_visitor
{
    Turns& m_turns; // because partition is currently operating on const input only
    Pieces const& m_pieces; // to check for piece-type

    template <typename Point>
    static inline bool projection_on_segment(Point const& subject, Point const& p, Point const& q)
    {
        typedef Point vector_type;
        typedef typename geometry::coordinate_type<Point>::type coordinate_type;

        vector_type v = q;
        vector_type w = subject;
        subtract_point(v, p);
        subtract_point(w, p);

        coordinate_type const zero = coordinate_type();
        coordinate_type const c1 = dot_product(w, v);

        if (c1 < zero)
        {
            return false;
        }
        coordinate_type const c2 = dot_product(v, v);
        if (c2 < c1)
        {
            return false;
        }

        return true;
    }

    template <typename Point, typename Piece>
    inline bool on_offsetted(Point const& point, Piece const& piece) const
    {
        typedef typename strategy::side::services::default_strategy
            <
                typename cs_tag<Point>::type
            >::type side_strategy;

        for (int i = 1; i < piece.offsetted_count; i++)
        {
            Point const& previous = piece.robust_ring[i - 1];
            Point const& current = piece.robust_ring[i];
            int const side = side_strategy::apply(point, previous, current);
            if (side == 0)
            {
                // Collinear, check if projection falls on it
                if (projection_on_segment(point, previous, current))
                {
                    return true;
                }
            }

        }
        return false;
    }

public:

    inline turn_in_piece_visitor(Turns& turns, Pieces const& pieces)
        : m_turns(turns)
        , m_pieces(pieces)
    {}

    template <typename Turn, typename Piece>
    inline void apply(Turn const& turn, Piece const& piece, bool first = true)
    {
        boost::ignore_unused_variable_warning(first);

        if (turn.count_within > 0)
        {
            // Already inside - no need to check again
            return;
        }

        if (piece.type == strategy::buffer::buffered_flat_end
            || piece.type == strategy::buffer::buffered_concave)
        {
            // Turns cannot be inside a flat end (though they can be on border)
            // Neither we need to check if they are inside concave helper pieces
            return;
        }

        bool neighbour = false;
        for (int i = 0; i < 2; i++)
        {
            // Don't compare against one of the two source-pieces
            if (turn.operations[i].piece_index == piece.index)
            {
                return;
            }

            typename boost::range_value<Pieces>::type const& pc
                                = m_pieces[turn.operations[i].piece_index];
            if (pc.left_index == piece.index
                || pc.right_index == piece.index)
            {
                if (pc.type == strategy::buffer::buffered_flat_end)
                {
                    // If it is a flat end, don't compare against its neighbor:
                    // it will always be located on one of the helper segments
                    return;
                }
                neighbour = true;
            }
        }

        int geometry_code = detail::within::point_in_geometry(turn.robust_point, piece.robust_ring);

        if (geometry_code == -1)
        {
            return;
        }

        Turn& mutable_turn = m_turns[turn.turn_index];
        if (geometry_code == 0 && ! neighbour)
        {
            // If it is on the border and they are neighbours, it should be
            // on the offsetted ring

            if (! on_offsetted(turn.robust_point, piece))
            {
                // It is on the border but not on the offsetted ring.
                // Then it is somewhere on the helper-segments
                // Classify it as inside
                geometry_code = 1;
                mutable_turn.count_on_helper++;
            }
        }

        switch (geometry_code)
        {
            case 1 : mutable_turn.count_within++; break;
            case 0 : mutable_turn.count_on_offsetted++; break;
        }
    }
};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR


#include "FixedAxisOneDoFJoint.h"
#include "drakeFloatingPointUtil.h"
#include <cmath>
#include <Eigen/Core>
#include <limits>
#include <exception>
#include <stdexcept>

using namespace Eigen;

FixedAxisOneDoFJoint::FixedAxisOneDoFJoint(const std::string& name, const Isometry3d& transform_to_parent_body, const Matrix<double, TWIST_SIZE, 1>& joint_axis) :
    DrakeJoint(name, transform_to_parent_body, 1, 1),
    joint_axis(joint_axis),
    joint_limit_min(-std::numeric_limits<double>::infinity()),
    joint_limit_max(std::numeric_limits<double>::infinity())
{
  // empty
}

FixedAxisOneDoFJoint::~FixedAxisOneDoFJoint()
{
  // empty
}

void FixedAxisOneDoFJoint::setJointLimits(double joint_limit_min, double joint_limit_max)
{
  if (joint_limit_min > joint_limit_max) {
    throw std::logic_error("joint_limit_min cannot be larger than joint_limit_max");
  }

  this->joint_limit_min = joint_limit_min;
  this->joint_limit_max = joint_limit_max;
}

void FixedAxisOneDoFJoint::motionSubspace(double* const q, MotionSubspaceType& motion_subspace, MatrixXd* dmotion_subspace) const
{
  motion_subspace = joint_axis;
  if (dmotion_subspace) {
    dmotion_subspace->setZero(motion_subspace.size(), getNumPositions());
  }
}

void FixedAxisOneDoFJoint::motionSubspaceDotTimesV(double* const q, double* const v,
    Vector6d& motion_subspace_dot_times_v,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdq,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdv) const
{
  motion_subspace_dot_times_v.setZero();

  if (dmotion_subspace_dot_times_vdq) {
    dmotion_subspace_dot_times_vdq->setZero(TWIST_SIZE, 1);
  }

  if (dmotion_subspace_dot_times_vdv) {
    dmotion_subspace_dot_times_vdv->setZero(TWIST_SIZE, 1);
  }
}

void FixedAxisOneDoFJoint::randomConfiguration(double* q, std::default_random_engine& generator) const
{
  if (isFinite(joint_limit_min) && isFinite(joint_limit_max)) {
    std::uniform_real_distribution<double> distribution(joint_limit_min, joint_limit_max);
    q[0] = distribution(generator);
  }
  else {
    std::normal_distribution<double> distribution;
    double stddev = 1.0;
    double joint_limit_offset = 1.0;
    if (isFinite(joint_limit_min)) {
      distribution = std::normal_distribution<double>(joint_limit_min + joint_limit_offset, stddev);
    }
    else if (isFinite(joint_limit_max)) {
      distribution = std::normal_distribution<double>(joint_limit_max - joint_limit_offset, stddev);
    }
    else {
      distribution = std::normal_distribution<double>();
    }

    q[0] = distribution(generator);
    if (q[0] < joint_limit_min) {
      q[0] = joint_limit_min;
    }
    if (q[0] > joint_limit_max) {
      q[0] = joint_limit_max;
    }
  }
}

void FixedAxisOneDoFJoint::qdot2v(double* q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());
  if (dqdot_to_v) {
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
  }
}

void FixedAxisOneDoFJoint::v2qdot(double* q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());
  if (dv_to_qdot) {
    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
  }
}


/*
 * Path finder file
 *
 * This file is part of the "SoftPixel Engine" (Copyright (c) 2008 by Lukas Hermanns)
 * See "SoftPixelEngine.hpp" for license information.
 */

#include "Framework/Tools/spToolPathFinder.hpp"

#ifdef SP_COMPILE_WITH_PATHFINDER

#include "Base/spMemoryManagement.hpp"

#include <boost/foreach.hpp>

namespace sp
{
namespace tool
{

/*
 * PathNode class
 */

PathNode::PathNode() :
    BaseObject      (   ),
    WayCosts_       (0  ),
    DirectDistance_ (0  ),
    Predecessor_    (0  )
{
}
PathNode::PathNode(const dim::vector3df &Position, void* Data) :
    BaseObject      (           ),
    Position_       (Position   ),
    WayCosts_       (0          ),
    DirectDistance_ (0          ),
    Predecessor_    (0          )
{
    setUserData(Data);
}
PathNode::~PathNode()
{
}

void PathNode::setPosition(const dim::vector3df &Position)
{
    Position_ = Position;

    foreach (PathEdge* Edge, Edges_)
        Edge->updateNodePosition(this);
}

std::list<PathNode*> PathNode::getNeighbors() const
{
    std::list<PathNode*> Neighbors;

    for (std::list<SNeighbor>::const_iterator it = Neighbors_.begin(); it != Neighbors_.end(); ++it)
        Neighbors.push_back(it->Node);

    return Neighbors;
}

void PathNode::addEdge(PathEdge* Edge)
{
    Edges_.push_back(Edge);
    updateNeighbors();
}
void PathNode::removeEdge(PathEdge* Edge)
{
    MemoryManager::removeElement(Edges_, Edge);
    updateNeighbors();
}

void PathNode::updateNeighbors()
{
    Neighbors_.clear();

    for (std::list<PathEdge*>::iterator it = Edges_.begin(); it != Edges_.end(); ++it)
    {
        if ((*it)->getAdjusted())
        {
            if ((*it)->getFrom() == this)
                Neighbors_.push_back(SNeighbor(this, (*it)->getTo()));
            else
                Neighbors_.push_back(SNeighbor(this, (*it)->getFrom()));
        }
        else
            Neighbors_.push_back(SNeighbor(this, (*it)->getTo()));
    }
}

PathNode::SNeighbor::SNeighbor(PathNode* Predecessor, PathNode* Neighbor) :
    Node    (Neighbor                                                               ),
    Distance(math::getDistance(Predecessor->getPosition(), Neighbor->getPosition()) )
{
}
PathNode::SNeighbor::~SNeighbor()
{
}

/*
 * PathEdge class
 */

PathEdge::PathEdge() :
    BaseObject  (       ),
    From_       (0      ),
    To_         (0      ),
    Distance_   (0      ),
    Adjusted_   (false  )
{
}
PathEdge::PathEdge(PathNode* From, PathNode* To, bool Adjusted) :
    BaseObject  (           ),
    From_       (From       ),
    To_         (To         ),
    Adjusted_   (Adjusted   )
{
    if (From_ && To_)
    {
        Distance_ = math::getDistance(From_->getPosition(), To_->getPosition());
        From_->addEdge(this);
        if (!Adjusted_)
            To_->addEdge(this);
    }
    else
    {
        From_ = To_ = 0;
        Adjusted = false;
    }
}
PathEdge::~PathEdge()
{
}

void PathEdge::updateNodePosition(PathNode* Node)
{
    if (From_ == Node)
        Distance_ = math::getDistance(Node->getPosition(), To_->getPosition());
    else
        Distance_ = math::getDistance(From_->getPosition(), Node->getPosition());
}

/*
 * PathFinder class
 */

PathGraph::PathGraph() :
    StartNode_  (0      ),
    TargetNode_ (0      ),
    isSolved_   (false  )
{
}
PathGraph::~PathGraph()
{
    MemoryManager::deleteList(NodeList_);
    MemoryManager::deleteList(EdgeList_);
}

PathNode* PathGraph::addNode(const dim::vector3df &Position, void* Data)
{
    PathNode* NewNode = new PathNode(Position, Data);
    NodeList_.push_back(NewNode);
    return NewNode;
}
void PathGraph::removeNode(PathNode* Node)
{
    /* Remove each edge which is incident to this node */
    for (std::list<PathEdge*>::iterator it = EdgeList_.begin(); it != EdgeList_.end();)
    {
        if ((*it)->getFrom() == Node || (*it)->getTo() == Node)
        {
            (*it)->getFrom()->removeEdge(*it);
            (*it)->getTo()->removeEdge(*it);

            MemoryManager::deleteMemory(*it);
            it = EdgeList_.erase(it);
        }
        else
            ++it;
    }

    /* Remove the node */
    MemoryManager::removeElement(NodeList_, Node, true);
}

void PathGraph::clearNodeList()
{
    MemoryManager::deleteList(NodeList_);
    MemoryManager::deleteList(EdgeList_);
}

PathEdge* PathGraph::addEdge(PathNode* From, PathNode* To, bool Adjusted)
{
    if (From && To && From != To)
    {
        PathEdge* NewEdge = new PathEdge(From, To, Adjusted);
        EdgeList_.push_back(NewEdge);
        return NewEdge;
    }
    return 0;
}
void PathGraph::removeEdge(PathEdge* Edge)
{
    MemoryManager::removeElement(EdgeList_, Edge);
}

void PathGraph::clearEdgeList()
{
    MemoryManager::deleteList(EdgeList_);
}

void PathGraph::createGrid(
    const dim::vector3df &From, const dim::vector3df &To, const dim::vector3di &Steps,
    const std::vector<bool> &Bitmap, bool DiagonalEdges)
{
    if (Steps.X < 1 || Steps.Y < 1 || Steps.Z < 1)
        return;

    s32 ArrayIndex;
    dim::vector3di i, j, k;
    dim::vector3df p;
    PathNode**** NodeVec = 0;

    const dim::vector3di Stretch(
        math::Max(1, Steps.X - 1), math::Max(1, Steps.Y - 1), math::Max(1, Steps.Z - 1)
    );

    /* Create nodes */
    NodeVec = new PathNode***[Steps.Z];
    for (i.Z = 0; i.Z < Steps.Z; ++i.Z)
    {
        NodeVec[i.Z] = new PathNode**[Steps.Y];
        for (i.Y = 0; i.Y < Steps.Y; ++i.Y)
        {
            NodeVec[i.Z][i.Y] = new PathNode*[Steps.X];
            for (i.X = 0; i.X < Steps.X; ++i.X)
            {
                ArrayIndex = i.Z*Steps.Y*Steps.X + i.Y*Steps.X + i.X;

                if (static_cast<u32>(ArrayIndex) >= Bitmap.size() || Bitmap[ArrayIndex])
                {
                    p = From + (To - From) * i.cast<f32>() / Stretch.cast<f32>();
                    NodeVec[i.Z][i.Y][i.X] = addNode(p);
                }
                else
                    NodeVec[i.Z][i.Y][i.X] = 0;
            }
        }
    }

    /* Create edges */
    for (i.Z = 0; i.Z < Steps.Z; ++i.Z)
    {
        for (i.Y = 0; i.Y < Steps.Y; ++i.Y)
        {
            for (i.X = 0; i.X < Steps.X; ++i.X)
            {
                /* Check all neighbors */
                for (j.Z = -1; j.Z <= 1; ++j.Z)
                for (j.Y = -1; j.Y <= 1; ++j.Y)
                for (j.X = -1; j.X <= 1; ++j.X)
                {
                    k = i + j;
                    if ( j != dim::vector3di(0) &&
                         k.X >= 0 && k.X < Steps.X &&
                         k.Y >= 0 && k.Y < Steps.Y &&
                         k.Z >= 0 && k.Z < Steps.Z )
                    {
                        if ( DiagonalEdges || ( j.X == 0 && j.Y == 0 ) || ( j.X == 0 && j.Z == 0 ) || ( j.Y == 0 && j.Z == 0 ) )
                            addEdge(NodeVec[i.Z][i.Y][i.X], NodeVec[k.Z][k.Y][k.X], true);
                    }
                }
            }
            //delete [] NodeVec[i.Z][i.Y];
        }
        //delete [] NodeVec[i.Z];
    }
    for (i.Z = 0; i.Z < Steps.Z; ++i.Z)
    {
        for (i.Y = 0; i.Y < Steps.Y; ++i.Y)
            delete [] NodeVec[i.Z][i.Y];
        delete [] NodeVec[i.Z];
    }
    delete [] NodeVec;
}

std::list<PathNode*> PathGraph::findPath(PathNode* From, PathNode* To)
{
    std::list<PathNode*> Path;
    findPath(From, To, Path);
    return Path;
}

bool PathGraph::findPath(PathNode* From, PathNode* To, std::list<PathNode*> &Path)
{
    /* Check parameter validity */
    if (!From || !To)
        return false;
    if (From == To)
    {
        Path.push_back(From);
        return true;
    }

    /* Initialization */
    StartNode_  = From;
    TargetNode_ = To;

    isSolved_ = false;

    /* Add the first node to the queue list */
    addNodeToQueue(StartNode_, 0, 0);

    /* Process next step while searching the optimal path in the graph */
    while (nextStep());

    /* Reset the storage */
    NodeQueue_.clear();
    ClosedMap_.clear();

    if (!isSolved_)
        return false;

    /* Build the path */
    constructPath(Path, TargetNode_);

    return true;
}

std::list<PathNode*> PathGraph::findPath(const dim::vector3df &From, const dim::vector3df &To)
{
    std::list<PathNode*> Path;
    findPath(From, To, Path);
    return Path;
}

bool PathGraph::findPath(const dim::vector3df &From, const dim::vector3df &To, std::list<PathNode*> &Path)
{
    PathNode* FromNode = 0, *ToNode = 0;

    f32 DistSq;
    f32 DistFrom = 999999.f, DistTo = 999999.f;

    foreach (PathNode* Node, NodeList_)
    {
        /* Check distance for start node */
        DistSq = math::getDistanceSq(From, Node->getPosition());

        if (DistSq < DistFrom)
        {
            DistFrom = DistSq;
            FromNode = Node;
        }

        /* Check distance for end node */
        DistSq = math::getDistanceSq(To, Node->getPosition());

        if (DistSq < DistTo)
        {
            DistTo = DistSq;
            ToNode = Node;
        }
    }

    return findPath(FromNode, ToNode, Path);
}

/*
 * ======= Protected: =======
 */

bool PathGraph::nextStep()
{
    /* Get the next accessable node */
    PathNode* CurNode = getNextNode();

    /* Check if no path has been found */
    if (!CurNode)
        return false;

    /* Check if path has been found */
    if (CurNode == TargetNode_)
    {
        isSolved_ = true;
        return false;
    }

    /* Add the neighbors to the queue */
    for (std::list<PathNode::SNeighbor>::iterator it = CurNode->Neighbors_.begin(); it != CurNode->Neighbors_.end(); ++it)
        addNodeToQueue(it->Node, CurNode, CurNode->WayCosts_ + it->Distance);

    return true;
}

PathNode* PathGraph::getNextNode()
{
    if (NodeQueue_.empty())
        return 0;

    /* Search the optimal next node */
    f32 Dist = 999999.f, CurDist;
    std::list<PathNode*>::iterator itLast = NodeQueue_.begin();

    for (std::list<PathNode*>::iterator it = NodeQueue_.begin(); it != NodeQueue_.end(); ++it)
    {
        CurDist = (*it)->getMinWayCosts();

        if (CurDist < Dist)
        {
            Dist    = CurDist;
            itLast  = it;
        }
    }

    /* Return the next node and remove it from the queue */
    PathNode* NextNode = *itLast;
    NodeQueue_.erase(itLast);

    return NextNode;
}

void PathGraph::addNodeToQueue(PathNode* Node, PathNode* Predecessor, const f32 WayCosts)
{
    /* Check if the node is already in the closed list */
    std::map<PathNode*, bool>::iterator itMap = ClosedMap_.find(Node);

    if (itMap == ClosedMap_.end())
    {
        /* Store the direct distance between the current node and the target node */
        Node->DirectDistance_ = math::getDistance(Node->getPosition(), TargetNode_->getPosition());

        /* Add the node to the queue and to the closed list */
        NodeQueue_.push_back(Node);
        ClosedMap_[Node] = true;
    }
    if (itMap == ClosedMap_.end() || WayCosts < Node->WayCosts_)
    {
        /* Setup information about this node */
        Node->Predecessor_  = Predecessor;
        Node->WayCosts_     = WayCosts;
    }
}

void PathGraph::constructPath(std::list<PathNode*> &Path, PathNode* NextNode)
{
    if (!NextNode)
        return;

    /* Add the first node to the path */
    Path.push_back(NextNode);

    /* Repeat the following process until the start node has reached */
    do
    {
        /* Go to the next predecessor */
        if (NextNode->Predecessor_)
            NextNode = NextNode->Predecessor_;
        else
        {
            io::Log::error("Incomplete path while searching in graph");
            break;
        }

        /* Add the next node to the path */
        Path.push_back(NextNode);
    }
    while (NextNode != StartNode_);
}

} // /namespace tool

} // /namespace sp

#endif

// ================================================================================


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE3_SUM_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE3_SUM_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE3_SUPPORT

#include <boost/simd/reduction/functions/sum.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/fusion/algorithm/iteration/fold.hpp>

namespace boost { namespace simd { namespace ext
{
  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is double
  /////////////////////////////////////////////////////////////////////////////
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sum_, boost::simd::tag::sse3_,
                      (A0),
                      ((simd_<double_<A0>,boost::simd::tag::sse_>))
                     )
  {
      typedef typename meta::scalar_of<A0>::type                 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1) {
      A0 b1 = _mm_hadd_pd (a0, Zero<A0>());
      double r;
      _mm_store_sd(&r, b1);
      return r;
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A0 is float
  /////////////////////////////////////////////////////////////////////////////
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::sum_, boost::simd::tag::sse3_,
                      (A0),
                      ((simd_<single_<A0>,boost::simd::tag::sse_>))
                     )
  {
      typedef typename meta::scalar_of<A0>::type   result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 b1 = _mm_hadd_ps (a0, Zero<A0>());
      b1 = _mm_hadd_ps (b1, Zero<A0>());
      float r;
      _mm_store_ss(&r, b1);
      return r;
    }
  };
} } }
#endif
#endif


#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/registration/icp.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl_conversions/pcl_conversions.h>
#include <boost/array.hpp>

//include for ROS
#include "ros/ros.h"
#include <tms_msg_ss/ods_get_robots_pos.h>
#include <tms_msg_ss/ods_pcd.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <tms_msg_rc/smartpal_control.h>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <stdlib.h>

ros::ServiceServer service;
ros::ServiceClient commander_to_kinect_capture;

int TABLE;

#define MAP_X 50
#define MAP_Y 50

typedef struct{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::PointCloud<pcl::PointXYZRGB> cloud_rgb;
    pcl::PointIndices inliers;
    pcl::PointXYZ g;
    int map[MAP_X][MAP_Y];
    int tf;
    double min_x;
    double max_x;
    double min_y;
    double max_y;
    double max_z;
    double min_z;
    int size_x;
    int size_y;
} object_map;

std::vector<object_map> Object_Map;

#define SMARTPAL4 0
#define SMARTPAL5 1
#define PI 3.1415926

double C_OFFSET_X;
double C_OFFSET_Y;
double C_OFFSET_Z;
double C_OFFSET_YAW;

typedef struct{
    double min_x;
    double max_x;
    double min_y;
    double max_y;
    double min_z;
    double max_z;
}Model;

Model m_size;

typedef struct{
    double x;
    double y;
    double theta;
}Robot;

typedef struct{
    double x;
    double y;
    double z;
}Furniture;

Robot robot;
Furniture table;

pcl::PointCloud<pcl::PointXYZRGB> viewpoint;

void transformation(pcl::PointCloud<pcl::PointXYZRGB>& cloud, pcl::PointCloud<pcl::PointXYZ>& model)
{
    std::cout << "transformation" << std::endl;

    double camera_x = robot.x + C_OFFSET_Y*cos(robot.theta) + C_OFFSET_X*sin(robot.theta);
    double camera_y = robot.y + C_OFFSET_Y*sin(robot.theta) - C_OFFSET_X*cos(robot.theta);
    double camera_z = C_OFFSET_Z;
    double camera_yaw = C_OFFSET_YAW*(PI/180.0);

    robot.theta *= PI/180.0;

    Eigen::Matrix4f t1(Eigen::Matrix4f::Identity());

    t1(0,0) = sin(robot.theta);    t1(0,1) = -sin(camera_yaw)*cos(robot.theta);    t1(0,2) = cos(camera_yaw)*cos(robot.theta);    t1(0,3) = camera_x;
    t1(1,0) = -cos(robot.theta);    t1(1,1) = -sin(camera_yaw)*sin(robot.theta);     t1(1,2) = cos(camera_yaw)*sin(robot.theta);     t1(1,3) = camera_y;
    t1(2,0) = 0;                    t1(2,1) = -cos(camera_yaw);                      t1(2,2) = -sin(camera_yaw);                     t1(2,3) = camera_z;
    t1(3,0) = 0;                    t1(3,1) = 0;                                    t1(3,2) = 0;                                    t1(3,3) = 1;

    pcl::transformPointCloud(viewpoint, viewpoint, t1);
    pcl::transformPointCloud(cloud, cloud, t1);

    t1(0,0) = 1;    t1(0,1) = 0;    t1(0,2) = 0;    t1(0,3) = table.x;
    t1(1,0) = 0;    t1(1,1) = 1;    t1(1,2) = 0;    t1(1,3) = table.y;
    t1(2,0) = 0;    t1(2,1) = 0;    t1(2,2) = 1;    t1(2,3) = table.z;
    t1(3,0) = 0;    t1(3,1) = 0;    t1(3,2) = 0;    t1(3,3) = 1;

    pcl::transformPointCloud(model, model, t1);

    return;
}

void filtering(pcl::PointCloud<pcl::PointXYZRGB>& cloud, pcl::PointCloud<pcl::PointXYZ>& model, pcl::PointIndices& inliers)
{
    std::cout << "filtering" << std::endl;

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

    m_size.min_x = 100.0;
    m_size.max_x = -100.0;
    m_size.min_y = 100.0;
    m_size.max_y = -100.0;
    m_size.min_z = 100.0;
    m_size.max_z = -100.0;

    for(int i=0;i<model.points.size();i++){
        if(model.points[i].x < m_size.min_x) m_size.min_x = model.points[i].x;
        if(model.points[i].x > m_size.max_x) m_size.max_x = model.points[i].x;
        if(model.points[i].y < m_size.min_y) m_size.min_y = model.points[i].y;
        if(model.points[i].y > m_size.max_y) m_size.max_y = model.points[i].y;
        if(model.points[i].z < m_size.min_z) m_size.min_z = model.points[i].z;
        if(model.points[i].z > m_size.max_z) m_size.max_z = model.points[i].z;
    }

    for(int i=0;i<cloud.points.size();i++){
        if(((m_size.min_x-0.3 <= cloud.points[i].x) && (cloud.points[i].x <= m_size.max_x+0.3)) &&
                ((m_size.min_y-0.3 <= cloud.points[i].y) && (cloud.points[i].y <= m_size.max_y+0.3)) &&
                ((m_size.min_z+0.05 <= cloud.points[i].z) && (cloud.points[i].z <= m_size.max_z+0.3))){
            tmp_cloud->push_back(cloud.points[i]);
            inliers.indices.push_back(i);
        }
    }

    pcl::copyPointCloud(*tmp_cloud, cloud);

    return;
}

//ダウンサンプリングを行う
void downsampling(pcl::PointCloud<pcl::PointXYZ>& cloud, float th)
{
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud.makeShared());
    sor.setLeafSize (th, th, th);
    sor.filter (cloud);

    return;
}

//マッチング
void registration(pcl::PointCloud<pcl::PointXYZ>& cloud, pcl::PointCloud<pcl::PointXYZ>& model, pcl::PointCloud<pcl::PointXYZ>& cloud_out, pcl::PointCloud<pcl::PointXYZRGB>& tmp_rgb, Eigen::Matrix4f& m)
{
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud.makeShared ());
    icp.setInputTarget(model.makeShared ());
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);

    m = icp.getFinalTransformation();
    //std::cout << m << std::endl;

    pcl::transformPointCloud (cloud, cloud, m);
    pcl::transformPointCloud (tmp_rgb, tmp_rgb, m);
    pcl::transformPointCloud(viewpoint, viewpoint, m);
    pcl::copyPointCloud(tmp_rgb, cloud_out);

    cloud_out += model;

    return;
}

///////////////////////////////////////////////////////
//*******************main function*******************//
///////////////////////////////////////////////////////

bool robot_position (tms_msg_ss::ods_get_robots_pos::Request &req, tms_msg_ss::ods_get_robots_pos::Response &res)
{
    //***************************
    // local variable declaration
    //***************************
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr model (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

    //***************************
    // capture kinect data
    //***************************
    tms_msg_ss::ods_pcd srv;
    srv.request.id = 3;
    if(commander_to_kinect_capture.call(srv)){
        pcl::fromROSMsg (srv.response.cloud, *cloud1);
    }

    if(SMARTPAL4){
        C_OFFSET_X = 0.02;
        C_OFFSET_Y = 0.14;
        C_OFFSET_Z = 1.38;
        C_OFFSET_YAW = 21.6;
    }

    else if(SMARTPAL5){
        C_OFFSET_X = 0.0;
        C_OFFSET_Y = 0.05;//0.01;
        C_OFFSET_Z = 1.05;
        C_OFFSET_YAW = 10.0;
    }

    viewpoint.resize(4);
    viewpoint[0].x = 0.0;  viewpoint[0].y = 0.0; viewpoint[0].z = 0.0;
    viewpoint[1].x = 1.0;  viewpoint[1].y = 0.0; viewpoint[1].z = 0.0;
    viewpoint[2].x = 0.0;  viewpoint[2].y = 1.0; viewpoint[2].z = 0.0;
    viewpoint[3].x = 0.0;  viewpoint[3].y = 0.0; viewpoint[3].z = 1.0;

    //std::cout << "viewpoint: " << viewpoint[0].x << " " << viewpoint[0].y << " " << viewpoint[0].z << std::endl;

    //pcl::io::loadPCDFile ("src/ods_robot_position/data/robot_position/table.pcd", *cloud1);
    //pcl::io::loadPCDFile ("src/ods_robot_position/data/robot_position/model_table1.pcd", *model);
    //pcl::io::loadPCDFile ("src/ods_robot_position/data/robot_position/model_shelf.pcd", *model);

    //***************************
    // Fill in the cloud data
    //***************************
    table.x = req.furniture.position.x/1000.0;
    table.y = req.furniture.position.y/1000.0;
    table.z = req.furniture.position.z/1000.0;
    robot.x = req.robot.x/1000.0;
    robot.y = req.robot.y/1000.0;
    robot.theta = req.robot.theta;

    /*tms_msg_rc::smartpal_control srv3;
    srv3.request.unit = 1;
    srv3.request.cmd = 8;
    if(commander_to_get_robots_info.call(srv3)){
        robots.x = srv3.response.val[0];
        robots.y = srv3.response.val[1];
        robots.theta = srv3.response.val[2];
    }*/

    std::cout << "table: " << table.x << " " << table.y << " " << table.z << std::endl;
    std::cout << "robot: " << robot.x << " " << robot.y << " " << robot.theta << std::endl;
    pcl::fromROSMsg (req.model, *model);
    //pcl::io::savePCDFile("src/ods_robot_position/data/robot_position/model.pcd", *model);

    //***************************
    // transform input cloud
    //***************************
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr tfm_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

    transformation(*cloud1, *model);

    pcl::copyPointCloud(*model, *tfm_cloud);

    for(int i=0;i<tfm_cloud->points.size();i++){
        tfm_cloud->points[i].r = 255;
        tfm_cloud->points[i].g = 255;
        tfm_cloud->points[i].b = 255;
    }

    for(int i=0;i<cloud1->points.size();i++){
        cloud1->points[i].r = 255;
        cloud1->points[i].g = 0;
        cloud1->points[i].b = 0;
    }

    *tfm_cloud += *cloud1;

    pcl::io::savePCDFile("src/ods_robot_position/data/robot_position/tfm_cloud1.pcd", *tfm_cloud);
    pcl::io::savePCDFile("src/rods_obot_position/data/robot_position/tfm_cloud2.pcd", *cloud1);

    //***************************
    // filtering by using model
    //***************************
    filtering(*cloud1, *model, *inliers);

    pcl::io::savePCDFile("src/ods_robot_position/data/robot_position/filter.pcd", *cloud1);

    pcl::PointCloud<pcl::PointXYZ>::Ptr dsp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloud1, *dsp_cloud);
    downsampling(*dsp_cloud, 0.02);

    //***************************
    // registration between two input pcd data
    //***************************
    Eigen::Matrix4f m1;
    std::cout << "registration" << std::endl;
    int n =0;
    while (1){
        // registration
        pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud (new pcl::PointCloud<pcl::PointXYZ>);
        registration(*dsp_cloud, *model, *m_cloud, *cloud1, m1);

        if((double)(m1(0,0)+m1(1,1)+m1(2,2)+m1(3,3)) >= 4){
            if(n > 2) break;
        }
        n++;
    }

    std::cout << viewpoint[0] << std::endl;

    res.m_robot.theta = atan2(viewpoint[3].y-viewpoint[0].y, viewpoint[3].x-viewpoint[0].x)*180.0/PI;
    res.m_robot.x = 1000.0*(viewpoint[0].x + C_OFFSET_X*cos((res.m_robot.theta-90.0)*PI/180) - C_OFFSET_Y*cos(res.m_robot.theta*PI/180));
    res.m_robot.y = 1000.0*(viewpoint[0].y + C_OFFSET_X*sin((res.m_robot.theta-90.0)*PI/180) - C_OFFSET_Y*sin(res.m_robot.theta*PI/180));

    std::cout << res.m_robot.x << " " << res.m_robot.y << " " << viewpoint[0].z - C_OFFSET_Z << " " << res.m_robot.theta << std::endl;

    FILE *fp;

    if((fp = fopen("src/ods_robot_position/data/robot_position/robot_position.txt", "a"))==NULL){
        std::cout << "file open error" << std::endl;
    }

    fprintf(fp, "%lf %lf %lf\n", res.m_robot.x, res.m_robot.y, res.m_robot.theta);

    fclose(fp);

    return true;
}

int main(int argc, char **argv)
{
    printf("init\n");
    ros::init(argc, argv, "ods_robot_position");
    ros::NodeHandle n;

    service = n.advertiseService("ods_robot_position", robot_position);

    commander_to_kinect_capture = n.serviceClient<tms_msg_ss::ods_pcd>("capture_cloud");

    ros::spin();

    return 0;
}


#include "globals.hpp"
#include "min_cut.cpp"
#include "read_bin_file.cpp"
#include "super_pixel.cpp"
#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

using namespace boost;

int main() {
  std::vector<BYTE> *imageFileData = readFile("assets/orange.bin");
  width = 320;
  height = 249;

  superpixel(*imageFileData);

  classPoints.push_back(ClassPoint{0, 100, 125});
  classPoints.push_back(ClassPoint{0, 160, 125});
  classPoints.push_back(ClassPoint{0, 180, 125});
  classPoints.push_back(ClassPoint{0, 130, 200});
  classPoints.push_back(ClassPoint{0, 67, 190});
  classPoints.push_back(ClassPoint{1, 10, 10});
  classPoints.push_back(ClassPoint{1, 100, 10});
  classPoints.push_back(ClassPoint{1, 150, 10});
  classPoints.push_back(ClassPoint{1, 240, 300});
  classPoints.push_back(ClassPoint{1, 100, 300});

  // classPoints.push_back(ClassPoint{0, 0, 0});
  // classPoints.push_back(ClassPoint{1, 0, 1});

  // numClusters = 2;
  // clusters.resize(1, 2);
  // clusters(0, 0) = 0;
  // clusters(0, 1) = 1;
  // clusters(1, 0) = 2;
  // clusters(1, 1) = 3;

  // spAdjMap.clear();
  // spAdjMap[0][1] = 0.2;
  // spAdjMap[1][0] = 0.2;
  // spAdjMap[0][2] = 0.3;
  // spAdjMap[1][2] = 0.5;
  // spAdjMap[1][3] = 0.5;
  // spAdjMap[2][3] = 0.7;

  prepareForMinCuts();
  minCutCls(0);
  minCutCls(1);
  resolveMasks();

  std::ofstream fout;
  fout.open("mincut.bin", std::ios::binary | std::ios::out);
  for (int i = 0; i < coloredMask.size(); i++) {
    fout.write((char *)&coloredMask[i], 4);
  }
  fout.close();

  return 0;
}


/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 *
 *************************************************************/

#ifndef _SV_REGION_HXX
#define _SV_REGION_HXX

#include <tools/gen.hxx>
#include <vcl/sv.h>
#include <vcl/dllapi.h>
#include <basegfx/polygon/b2dpolypolygon.hxx>
#include <boost/shared_ptr.hpp>

class ImplRegionBand;
class RegionBand;
class Polygon;
class PolyPolygon;

//////////////////////////////////////////////////////////////////////////////

typedef boost::shared_ptr< RegionBand > RegionBandPtr;
typedef boost::shared_ptr< PolyPolygon > PolyPolygonPtr;
typedef boost::shared_ptr< basegfx::B2DPolyPolygon > B2DPolyPolygonPtr;
typedef std::vector< Rectangle > RectangleVector;

//////////////////////////////////////////////////////////////////////////////

class VCL_DLLPUBLIC Region
{
private:
	friend class OutputDevice;
	friend class Window;
	friend class Bitmap;

    // possible contents
    B2DPolyPolygonPtr           mpB2DPolyPolygon;
    PolyPolygonPtr              mpPolyPolygon;
    RegionBandPtr               mpRegionBand;

    /// bitfield
    bool                        mbIsNull : 1;

    // helpers
    SAL_DLLPRIVATE void ImplCreatePolyPolyRegion( const PolyPolygon& rPolyPoly );
    SAL_DLLPRIVATE void ImplCreatePolyPolyRegion( const basegfx::B2DPolyPolygon& rPolyPoly );

    SAL_DLLPRIVATE PolyPolygon ImplCreatePolyPolygonFromRegionBand() const;
    SAL_DLLPRIVATE basegfx::B2DPolyPolygon ImplCreateB2DPolyPolygonFromRegionBand() const;

public:
	Region(bool bIsNull = false); // default creates empty region, with true a null region is created
	Region(const Rectangle& rRect);
	Region(const Polygon& rPolygon);
	Region(const PolyPolygon& rPolyPoly);
	Region(const basegfx::B2DPolyPolygon&);
	Region(const Region& rRegion);
	~Region();

    // direct access to contents
    const basegfx::B2DPolyPolygon* getB2DPolyPolygon() const { return mpB2DPolyPolygon.get(); }
    const PolyPolygon* getPolyPolygon() const { return mpPolyPolygon.get(); }
    const RegionBand* getRegionBand() const { return mpRegionBand.get(); }

    // access with converters, the asked data will be created from the most
    // valuable data, buffered and returned
    const PolyPolygon GetAsPolyPolygon() const;
    const basegfx::B2DPolyPolygon GetAsB2DPolyPolygon() const;
    const RegionBand* GetAsRegionBand() const;

    // manipulators
	void Move( long nHorzMove, long nVertMove );
	void Scale( double fScaleX, double fScaleY );
	bool Union( const Rectangle& rRegion );
	bool Intersect( const Rectangle& rRegion );
	bool Exclude( const Rectangle& rRegion );
	bool XOr( const Rectangle& rRegion );
	bool Union( const Region& rRegion );
	bool Intersect( const Region& rRegion );
	bool Exclude( const Region& rRegion );
	bool XOr( const Region& rRegion );

	bool IsEmpty() const;
	bool IsNull() const;

	void SetEmpty();
	void SetNull();

	Rectangle GetBoundRect() const;
    bool HasPolyPolygonOrB2DPolyPolygon() const { return (getB2DPolyPolygon() || getPolyPolygon()); }
    void GetRegionRectangles(RectangleVector& rTarget) const;

	bool IsInside( const Point& rPoint ) const;
	bool IsInside( const Rectangle& rRect ) const;
	bool IsOver( const Rectangle& rRect ) const;

	Region& operator=( const Region& rRegion );
	Region& operator=( const Rectangle& rRect );

	bool operator==( const Region& rRegion ) const;
	bool operator!=( const Region& rRegion ) const { return !(Region::operator==( rRegion )); }

	friend VCL_DLLPUBLIC SvStream& operator>>( SvStream& rIStm, Region& rRegion );
	friend VCL_DLLPUBLIC SvStream& operator<<( SvStream& rOStm, const Region& rRegion );

    /* workaround: faster conversion for PolyPolygons
     * if half of the Polygons contained in rPolyPoly are actually
     * rectangles, then the returned Region will be constructed by
     * XOr'ing the contained Polygons together; in the case of
     * only Rectangles this can be up to eight times faster than
     * Region( const PolyPolygon& ).
     * Caution: this is only useful if the Region is known to be
     * changed to rectangles; e.g. if being set as clip region
     */
    static Region GetRegionFromPolyPolygon( const PolyPolygon& rPolyPoly );
};

#endif	// _SV_REGION_HXX

//////////////////////////////////////////////////////////////////////////////
// eof