#include <boost/python.hpp>
#include <cstdlib>
#include <cstdio>
#include <cmath>
#include <algorithm>

#include "functional.h"
#include "time.h"

using namespace boost::python;
using namespace std;

namespace boost_functional
{

    class cDet
    {
    public:
        cDet() { type = eval("float"); }
        cDet(float x, float y, float w, float h) : x(x), y(y), w(w), h(h)
        {
            type = eval("float");
        }
        float _intersection(const cDet &o) const
        {
            return intersect(x, y, w, h, o.x, o.y, o.w, o.h);
        }
        float _area() const
        {
            return max(w, 0.f) * max(h, 0.f);
        }
        float _union(const cDet &o) const
        {
            float i = intersect(x, y, w, h, o.x, o.y, o.w, o.h);
            float a1 = _area();
            float a2 = o._area();
            return a1 + a2 - i;
        }
        float _iou(const cDet &o) const
        {
            float i = intersect(x, y, w, h, o.x, o.y, o.w, o.h);
            float a1 = _area();
            float a2 = o._area();
            return i / (a1 + a2 - i);
        }
        void _trim(const int w_, const int h_)
        {
            float x1 = x;
            float y1 = y;
            x = max(x, 0.f);
            y = max(y, 0.f);
            float x2 = min((float)w_, x1 + w);
            float y2 = min((float)h_, y1 + h);
            w = max(x2 - x, 0.f);
            h = max(y2 - y, 0.f);
        }
        object get_x1() const
        {
            return type(x);
        }
        object get_y1() const
        {
            return type(y);
        }
        object get_x2() const
        {
            return type(x + w);
        }
        object get_y2() const
        {
            return type(y + h);
        }
        object get_w() const
        {
            return type(w);
        }
        object get_h() const
        {
            return type(h);
        }
        object get_cx() const
        {
            return type(x + w / 2);
        }
        object get_cy() const
        {
            return type(y + h / 2);
        }
        void set_x1(object v)
        {
            x = extract<float>(v);
        }
        void set_y1(object v)
        {
            y = extract<float>(v);
        }
        void set_x2(object v)
        {
            w = extract<float>(v) - x;
        }
        void set_y2(object v)
        {
            h = extract<float>(v) - y;
        }
        void set_w(object v)
        {
            w = extract<float>(v);
        }
        void set_h(object v)
        {
            h = extract<float>(v);
        }
        void set_cx(object v)
        {
            x = extract<float>(v) - w / 2;
        }
        void set_cy(object v)
        {
            y = extract<float>(v) - h / 2;
        }
        object get_type() const
        {
            return type;
        }
        void set_type(object v)
        {
            type = v;
        }

    private:
        float x, y, w, h;
        object type;
    };

    struct cDet_picle_suite : pickle_suite
    {
        static tuple getinitargs(const cDet &v)
        {
            return make_tuple(v.get_x1(), v.get_y1(), v.get_w(), v.get_h());
        }
        static tuple getstate(object v)
        {
            cDet const &v_ = extract<cDet const &>(v);
            return make_tuple(v.attr("__dict__"), v_.get_type());
        }
        static void setstate(object v, tuple state)
        {
            cDet &v_ = extract<cDet &>(v);
            if (len(state) != 2)
            {
                PyErr_SetObject(PyExc_ValueError, ("expected 2-item tuple, got %s" % state).ptr());
                throw_error_already_set();
            }
            dict d = extract<dict>(v.attr("__dict__"));
            d.update(state[0]);

            object dtype = state[1];
            v_.set_type(dtype);
        }
        static bool getstate_manages_dict() { return true; }
    };

    str version()
    {
        return "senseTookit boost c extension (2019.12.02.0)";
    };

    object iou_batch(object adets, object bdets)
    {
        object &al = adets;
        object &bl = bdets;
        list ret;
        ssize_t cnt = extract<ssize_t>(al.attr("__len__")());
        if (extract<ssize_t>(al.attr("__len__")()) != cnt)
        {
            return ret;
        }
        for (ssize_t i = 0; i < cnt; ++i)
        {
            cDet &a = extract<cDet &>(al[i]);
            cDet &b = extract<cDet &>(bl[i]);
            ret.append(a._iou(b));
        }
        return ret;
    };

    object test(int x)
    {
        list ret;
        for (int i = 0; i < x; ++i)
            ret.append(i + 1);
        return ret;
    }

    object c_sum(object x)
    {
        object ret(0);
        ssize_t cnt = extract<ssize_t>(x.attr("__len__")());
        for (int i = 0; i < cnt; ++i)
            ret += x[i];
        return ret;
    }

    object expr(object x, object y)
    {
        return y(x);
    }
} // namespace boost_functional

BOOST_PYTHON_MODULE(boost_functional)
{
    using namespace boost_functional;
    class_<cDet>("cDet", init<>())
        .add_property("x1", &cDet::get_x1, &cDet::set_x1)
        .add_property("y1", &cDet::get_y1, &cDet::set_y1)
        .add_property("x2", &cDet::get_x2, &cDet::set_x2)
        .add_property("y2", &cDet::get_y2, &cDet::set_y2)
        .add_property("w", &cDet::get_w, &cDet::set_w)
        .add_property("h", &cDet::get_h, &cDet::set_h)
        .add_property("cx", &cDet::get_cx, &cDet::set_cx)
        .add_property("cy", &cDet::get_cy, &cDet::set_cy)
        .add_property("dtype", &cDet::get_type)
        .def(init<float, float, float, float>())
        .def("intersection", &cDet::_intersection)
        .def("area", &cDet::_area)
        .def("iou", &cDet::_iou)
        .def("union", &cDet::_union)
        .def("_trim", &cDet::_trim)
        .def("_astype", &cDet::set_type)

        .def_pickle(cDet_picle_suite());
    def("c_version", version);
    def("c_test", test);
    def("c_sum", c_sum);
    def("c_iou_batch", iou_batch);
    def("c_expr", expr);
}


//------------------------------------------------------------------------------
/*
    This file is part of Beast: https://github.com/vinniefalco/Beast
    Copyright 2013, Vinnie Falco <vinnie.falco@gmail.com>

    Permission to use, copy, modify, and/or distribute this software for any
    purpose  with  or without fee is hereby granted, provided that the above
    copyright notice and this permission notice appear in all copies.

    THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
    WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
    MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
    ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
    WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
    ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

#ifndef BEAST_WEBSOCKETDETAIL_MASKGEN_HPP
#define BEAST_WEBSOCKETDETAIL_MASKGEN_HPP

#include <boost/asio/buffer.hpp>
#include <array>
#include <climits>
#include <cstdint>
#include <random>
#include <type_traits>

namespace beast {
namespace websocket {
namespace detail {

// Pseudo-random source of mask keys
//
template<class = void>
class maskgen_t
{
    std::mt19937 g_;

public:
    using result_type = typename std::mt19937::result_type;

    maskgen_t(maskgen_t const&) = delete;
    maskgen_t& operator=(maskgen_t const&) = delete;

    maskgen_t();

    result_type
    operator()() noexcept;

    void
    rekey();
};

template<class _>
maskgen_t<_>::maskgen_t()
{
    rekey();
}

template<class _>
auto
maskgen_t<_>::operator()() noexcept ->
    result_type
{
    for(;;)
        if(auto key = g_())
            return key;
}

template<class _>
void
maskgen_t<_>::rekey()
{
    std::random_device rng;
    std::array<std::uint32_t, 32> e;
    for(auto& i : e)
        i = rng();
    std::seed_seq ss(e.begin(), e.end());
    g_.seed(ss);
}

using maskgen = maskgen_t<>;

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

//using prepared_key_type = std::size_t;
using prepared_key_type = std::uint32_t;
//using prepared_key_type = std::uint64_t;

inline
void
prepare_key(std::uint32_t& prepared, std::uint32_t key)
{
    prepared = key;
}

inline
void
prepare_key(std::uint64_t& prepared, std::uint32_t key)
{
    prepared =
        (static_cast<std::uint64_t>(key) << 32) | key;
}

template<class T>
inline
typename std::enable_if<std::is_integral<T>::value, T>::type
rol(T t, unsigned n = 1)
{
    auto constexpr bits =
        static_cast<unsigned>(
            sizeof(T) * CHAR_BIT);
    n &= bits-1;
    return static_cast<T>((t << n) | (
        static_cast<typename std::make_unsigned<T>::type>(t) >> (bits - n)));
}

template <class T>
inline
typename std::enable_if<std::is_integral<T>::value, T>::type
ror(T t, unsigned n = 1)
{
    auto constexpr bits =
        static_cast<unsigned>(
            sizeof(T) * CHAR_BIT);
    n &= bits-1;
    return static_cast<T>((t << (bits - n)) | (
        static_cast<typename std::make_unsigned<T>::type>(t) >> n));
}

// 32-bit Uuoptimized
//
template<class = void>
void
mask_inplace_safe(
    boost::asio::mutable_buffer const& b,
        std::uint32_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    for(auto i = n / sizeof(key); i; --i)
    {
        *p ^=  key      ; ++p;
        *p ^= (key >> 8); ++p;
        *p ^= (key >>16); ++p;
        *p ^= (key >>24); ++p;
    }
    n %= sizeof(key);
    switch(n)
    {
    case 3: p[2] ^= (key >>16);
    case 2: p[1] ^= (key >> 8);
    case 1: p[0] ^=  key;
        key = ror(key, n*8);
    default:
        break;
    }
}

// 64-bit unoptimized
//
template<class = void>
void
mask_inplace_safe(
    boost::asio::mutable_buffer const& b,
        std::uint64_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    for(auto i = n / sizeof(key); i; --i)
    {
        *p ^=  key      ; ++p;
        *p ^= (key >> 8); ++p;
        *p ^= (key >>16); ++p;
        *p ^= (key >>24); ++p;
        *p ^= (key >>32); ++p;
        *p ^= (key >>40); ++p;
        *p ^= (key >>48); ++p;
        *p ^= (key >>56); ++p;
    }
    n %= sizeof(key);
    switch(n)
    {
    case 7: p[6] ^= (key >>16);
    case 6: p[5] ^= (key >> 8);
    case 5: p[4] ^=  key;
    case 4: p[3] ^= (key >>24);
    case 3: p[2] ^= (key >>16);
    case 2: p[1] ^= (key >> 8);
    case 1: p[0] ^=  key;
        key = ror(key, n*8);
    default:
        break;
    }
}

// 32-bit optimized
template<class = void>
void
mask_inplace_32(
    boost::asio::mutable_buffer const& b,
        std::uint32_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    auto m = reinterpret_cast<
        uintptr_t>(p) % sizeof(key);
    switch(m)
    {
    case 1: *p ^=  key      ; ++p; --n;
    case 2: *p ^= (key >> 8); ++p; --n;
    case 3: *p ^= (key >>16); ++p; --n;
        key = ror(key, m * 8);
    case 0:
        break;
    }
    for(auto i = n / sizeof(key); i; --i)
    {
        *reinterpret_cast<
            std::uint32_t*>(p) ^= key;
        p += sizeof(key);
    }
    n %= sizeof(key);
    switch(n)
    {
    case 3: p[2] ^= (key >>16);
    case 2: p[1] ^= (key >> 8);
    case 1: p[0] ^=  key;
        key = ror(key, n*8);
    default:
        break;
    }
}

// 64-bit optimized
//
template<class = void>
void
mask_inplace_64(
    boost::asio::mutable_buffer const& b,
        std::uint64_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    auto m = reinterpret_cast<
        uintptr_t>(p) % sizeof(key);
    switch(m)
    {
    case 1: *p ^=  key      ; ++p; --n;
    case 2: *p ^= (key >> 8); ++p; --n;
    case 3: *p ^= (key >>16); ++p; --n;
    case 4: *p ^= (key >>24); ++p; --n;
    case 5: *p ^= (key >>32); ++p; --n;
    case 6: *p ^= (key >>40); ++p; --n;
    case 7: *p ^= (key >>48); ++p; --n;
        key = ror(key, m * 8);
    case 0:
        break;
    }
    for(auto i = n / sizeof(key); i; --i)
    {
        *reinterpret_cast<
            std::uint64_t*>(p) ^= key;
        p += sizeof(key);
    }
    n %= sizeof(key);
    switch(n)
    {
    case 3: p[2] ^= (key >>16);
    case 2: p[1] ^= (key >> 8);
    case 1: p[0] ^=  key;
        key = ror(key, n*8);
    default:
        break;
    }
}

// 32-bit x86 optimized
//
template<class = void>
void
mask_inplace_x86(
    boost::asio::mutable_buffer const& b,
        std::uint32_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    for(auto i = n / sizeof(key); i; --i)
    {
        *reinterpret_cast<
            std::uint32_t*>(p) ^= key;
        p += sizeof(key);
    }
    n %= sizeof(key);
    switch(n)
    {
    case 3: p[2] ^= (key >>16);
    case 2: p[1] ^= (key >> 8);
    case 1: p[0] ^=  key;
        key = ror(key, n*8);
    default:
        break;
    }
}

// 64-bit amd64 optimized
//
template<class = void>
void
mask_inplace_amd(
    boost::asio::mutable_buffer const& b,
        std::uint64_t& key)
{
    using boost::asio::buffer_cast;
    using boost::asio::buffer_size;
    auto n = buffer_size(b);
    auto p = buffer_cast<std::uint8_t*>(b);
    for(auto i = n / sizeof(key); i; --i)
    {
        *reinterpret_cast<
            std::uint64_t*>(p) ^= key;
        p += sizeof(key);
    }
    n %= sizeof(key);
    switch(n)
    {
    case 7: p[6] ^= (key >>16);
    case 6: p[5] ^= (key >> 8);
    case 5: p[4] ^=  key;
    case 4: p[3] ^= (key >>24);
    case 3: p[2] ^= (key >>16);
    case 2: p[1] ^= (key >> 8);
    case 1: p[0] ^=  key;
        key = ror(key, n*8);
    default:
        break;
    }
}

inline
void
mask_inplace(
    boost::asio::mutable_buffer const& b,
        std::uint32_t& key)
{
    mask_inplace_safe(b, key);
    //mask_inplace_32(b, key);
    //mask_inplace_x86(b, key);
}

inline
void
mask_inplace(
    boost::asio::mutable_buffer const& b,
        std::uint64_t& key)
{
    mask_inplace_safe(b, key);
    //mask_inplace_64(b, key);
    //mask_inplace_amd(b, key);
}

// Apply mask in place
//
template<class MutableBuffers, class KeyType>
void
mask_inplace(
    MutableBuffers const& bs, KeyType& key)
{
    for(auto const& b : bs)
        mask_inplace(b, key);
}

} // detail
} // websocket
} // beast

#endif


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SELSUB_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SELSUB_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/if_else_zero.hpp>
#include <boost/simd/function/simd/unary_minus.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/plus.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( selsub_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::fundamental_<A0>, X>
                          , bs::pack_<bd::unspecified_<A1>, X>
                          , bs::pack_<bd::unspecified_<A1>, X>
                          )
   {
     BOOST_FORCEINLINE A1 operator()(A0 const& a0, A1 const& a1, A1 const& a2) const
      {
       return a1 - if_else_zero(a0, a2);
      }
   };

   BOOST_DISPATCH_OVERLOAD( selsub_
                           , (typename A0, typename A1, typename X)
                           , bd::cpu_
                           , bs::pack_<bd::fundamental_<A0>, X>
                           , bs::pack_<bd::floating_<A1>, X>
                           , bs::pack_<bd::floating_<A1>, X>
                          )
   {
     BOOST_FORCEINLINE A1 operator()(A0 const& a0, A1 const& a1, A1 const& a2) const
      {
        // this is a workaround for a gcc (at least 4.6) over-optimization in case or a1 and a2 are
        // equal (constant?) and invalid (inf -inf or nan) in which case the general impl sometimes
        // return 0 in place of nan in float cases.
       return a1 + if_else_zero(a0, -a2);
      }
   };
} } }

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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_IEEE_FUNCTIONS_FAST_LDEXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_FAST_LDEXP_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief fast_ldexp generic tag

     Represents the fast_ldexp function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct fast_ldexp_ : ext::elementwise_<fast_ldexp_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<fast_ldexp_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_fast_ldexp_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::fast_ldexp_, Site> dispatching_fast_ldexp_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::fast_ldexp_, Site>();
   }
   template<class... Args>
   struct impl_fast_ldexp_;
  }
  /*!
    The function multiply a floating entry \f$a_0\f$
    by \f$2^{a_1}\f$

    @par Semantic:

    @code
    T r = fast_ldexp(a0,a1);
    @endcode

    is similar to:

    @code
    T r =  a0*pow(2, a1);
    @endcode

    @par Note:
    The fast prefix indicates that for speed sake
    the result may be incorrect for limiting values:
    inf, -inf nan and zero that deserve special treatment.If you are not sure use @funcref{ldexp}, at the expense
    of some more machine cycles.

    @see  @funcref{ldexp}
    @param a0

    @param a1

    @return a value of same type as the inputs
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fast_ldexp_, fast_ldexp, 2)
} }

#endif


#pragma once

#ifdef USE_FIBONACCI_HEAP
#include <boost/heap/fibonacci_heap.hpp>
#endif

#include <unordered_map>
#include <unordered_set>

#include "a_star_jpssipp.hpp"

namespace libMultiRobotPlanning {

/*!
  \example sipp.cpp Simple example using a 2D grid world and
  up/down/left/right
  actions
*/

/*! \brief JPSSIPP Algorithm to find the shortest path with dynamic obstacles using the jump point search.

This class implements the SIPP algorithm. SIPP is an informed search algorithm
that finds the shortest path for a given map and dynamic a-priori known
obstacles.
It can use a heuristic that needs to be admissible.

Details of the algorithm can be found in the following paper:\n
Mike Phillips and Maxim Likhachev:\n
"SIPP:  Safe  Interval  Path  Planning  for  Dynamic  Environments". IEEE
International Conference on Robotics and Automation (ICRA), 2011\n
https://doi.org/10.1109/ICRA.2011.5980306

This class can either use a fibonacci heap, or a d-ary heap. The latter is the
default. Define "USE_FIBONACCI_HEAP" to use the fibonacci heap instead.

\tparam State Custom state for the search. Needs to be copy'able
\tparam Location Custom location type for the search. Needs to be copy'able
\tparam Cost Custom Cost type (integer or floating point types)
\tparam Environment This class needs to provide the custom A* logic. In
    particular, it needs to support the following functions:
  - `Cost admissibleHeuristic(const State& s)`\n
    This function can return 0 if no suitable heuristic is available.

  - `bool isSolution(const State& s)`\n
    Return true if the given state is a goal state.

  - `void getNeighbors(const State& s, std::vector<Neighbor<State, Action,
   int> >& neighbors)`\n
    Fill the list of neighboring state for the given state s.

  - `void onExpandNode(const State& s, int fScore, int gScore)`\n
    This function is called on every expansion and can be used for statistical
purposes.

  - `void onDiscover(const State& s, int fScore, int gScore)`\n
    This function is called on every node discovery and can be used for
   statistical purposes.
*/

template <typename State, typename Location, typename Action, typename Cost,
          typename Environment>
class JPSSIPP {
 public:
  struct interval {
    interval(Cost start, Cost end) : start(start), end(end) {}

    Cost start;
    Cost end;
    friend bool operator<(const interval& a, const interval& b) {
      return a.start < b.start;
    }
  };
public:
  struct edgeCollision{
	  edgeCollision(Cost t, Action action) : t(t), action(action){}

	Cost t;
	Action action;
	friend bool operator==(const edgeCollision& a, const edgeCollision& b){
		return (a.t == b.t) && (a.action == b.action);
	}
  };

public:
  struct startTime{
	  startTime(Cost rt, Action action, unsigned int dir, bool flag_wait) : rt(rt), action(action), dir(dir), flag_wait(flag_wait){}
	  Cost rt;
	  Action action;
	  bool flag_wait;
	  unsigned int dir;
	  friend bool operator<(const startTime&a, const startTime&b){
		  return a.rt < b.rt;
	  }
  };

 public:
  JPSSIPP(Environment& environment) : m_env(environment), m_astar(m_env) {}

  void setCollisionIntervals(const Location& location,
                             const std::vector<interval>& intervals) {
    m_env.setCollisionIntervals(location, intervals);
  }

  void setEdgeCollisions(const Location& location,
                             const std::vector<edgeCollision>& ec) {
    m_env.setEdgeCollisions(location, ec);
  }

  bool mightHaveSolution(const State& goal) {
    return m_env.mightHaveSolution(goal);
  }

  bool search(const State& startState, const Action& waitAction,
              PlanResult<State, Action, Cost>& solution, Cost startTime = 0) {
    PlanResult<JPSSIPPState, JPSSIPPAction, Cost> astarsolution;
    solution.cost = 0;
    solution.fmin = 0;
    solution.actions.clear();
    solution.states.clear();
    size_t interval;

    if (!m_env.findSafeInterval(startState, startTime, interval)) {
      return false;
    }
    bool success = m_astar.search(JPSSIPPState(startState, interval, 0xf, 0, false), astarsolution, startTime);

    solution.cost = astarsolution.cost - startTime;
    solution.fmin = astarsolution.fmin;
    for (size_t i = 0; i < astarsolution.actions.size(); ++i) {
      Cost waitTime =
          astarsolution.actions[i].second - astarsolution.actions[i].first.time;
      if (waitTime == 0) {
        solution.states.push_back(
            std::make_pair<>(astarsolution.states[i].first.state,
                             astarsolution.states[i].second));
        solution.actions.push_back(
            std::make_pair<>(astarsolution.actions[i].first.action,
                             astarsolution.actions[i].second));
      } else {
        // additional wait action before
        solution.states.push_back(
            std::make_pair<>(astarsolution.states[i].first.state,
                             astarsolution.states[i].second));
        solution.actions.push_back(std::make_pair<>(waitAction, waitTime));
        solution.states.push_back(
            std::make_pair<>(astarsolution.states[i].first.state,
                             astarsolution.states[i].second + waitTime));
        solution.actions.push_back(
            std::make_pair<>(astarsolution.actions[i].first.action,
                             astarsolution.actions[i].first.time));
      }
    }
    solution.states.push_back(
        std::make_pair<>(astarsolution.states.back().first.state,
                         astarsolution.states.back().second));

    return success;
  }

 private:
  // public:
  struct JPSSIPPState {
    JPSSIPPState(const State& state, size_t interval)
        : state(state), interval(interval), dir(0x00), g_cost(0), flag_wait(false){}

    JPSSIPPState(const State& state, size_t interval, unsigned int dir)
        : state(state), interval(interval), dir(dir), g_cost(0), flag_wait(false){}

    JPSSIPPState(const State& state, size_t interval, unsigned int dir,Cost g_cost, bool flag_wait)
        : state(state), interval(interval), dir(dir), g_cost(g_cost), flag_wait(flag_wait){}

    bool operator==(const JPSSIPPState& other) const {
      return std::tie(state, interval) == std::tie(other.state, other.interval);
    }

    friend std::ostream& operator<<(std::ostream& os, const JPSSIPPState& s) {
      return os << "(" << s.state << "," << s.interval << ")";
    }

//    Cost cost;
  public:
    Cost g_cost;
    State state;
    unsigned int dir = 0xf;
    bool flag_wait = false;
    Action action;
    size_t interval;
  };

  struct JPSSIPPStateHasher {
    size_t operator()(const JPSSIPPState& s) const {
      size_t seed = 0;
      boost::hash_combine(seed, std::hash<State>()(s.state));
      boost::hash_combine(seed, s.interval);
      return seed;
    }
  };

  struct JPSSIPPStateHasherOpen {
    size_t operator()(const JPSSIPPState& s) const {
      size_t seed = 0;
      boost::hash_combine(seed, std::hash<State>()(s.state));
      boost::hash_combine(seed, s.g_cost);
      return seed;
    }
  };

  struct JPSSIPPAction {
    JPSSIPPAction(const Action& action, Cost time) : action(action), time(time) {}
    Action action;
    Cost time;
  };

  // private:
  struct JPSSIPPEnvironment {
    JPSSIPPEnvironment(Environment& env) : m_env(env) {}

    Cost admissibleHeuristic(const JPSSIPPState& s) {
      return m_env.admissibleHeuristic(s.state);
    }

    bool mightHaveSolution(const State& goal) {
      const auto& si = safeIntervals(m_env.getLocation(goal));
      return m_env.isSolution(goal) &&
        !si.empty() &&
        si.back().end == std::numeric_limits<Cost>::max();
    }

    bool isSolution(const JPSSIPPState& s) {
      return m_env.isSolution(s.state) &&
             safeIntervals(m_env.getLocation(s.state)).at(s.interval).end ==
                 std::numeric_limits<Cost>::max();
    }

       void getNeighbors(
              const JPSSIPPState& s,
              std::vector<Neighbor<JPSSIPPState, JPSSIPPAction, Cost> >& neighbors) {

            std::vector<Neighbor<State, Action, Cost> > motions;

            JPSSIPPState s_temp = s;

            if(!m_env.isJPS()){
          	  m_env.getNeighbors(s_temp.state, motions);
       //   	  std::cout << "The number of neighbors: " << motions.size() << " " << std::endl;
                for (const auto& m : motions) {
              	  m_env.num_generation++;
      //        	  std::cout << "Current successor: " << m.state.x << " " << m.state.y << "\n";
                  Cost m_time = m.cost;
                  Cost start_t = m_lastGScore + m_time;
                  Cost end_t =
                      safeIntervals(m_env.getLocation(s.state)).at(s.interval).end;

                  const auto& sis = safeIntervals(m_env.getLocation(m.state));
                  for (size_t i = 0; i < sis.size(); ++i) {
                    const interval& si = sis[i];
                    if (si.start - m_time > end_t || si.end < start_t) {
                      continue;
                    }
                    int t;
                    Action a_temp;
                    if(m.action == Action::Left) a_temp = Action::Right;
                    else if(m.action == Action::Right) a_temp = Action::Left;
                    else if(m.action == Action::Up) a_temp = Action::Down;
                    else if(m.action == Action::Down) a_temp = Action::Up;
                    if (m_env.isCommandValid(s.state, m.state, m.action, m_lastGScore,
                                             end_t, si.start, si.end, t, m_time)
                    		&& !IsEdgeCollisions(m.state, edgeCollision(t - 1, a_temp))) {
                      neighbors.emplace_back(Neighbor<JPSSIPPState, JPSSIPPAction, Cost>(
                          JPSSIPPState(m.state, i,0xf), JPSSIPPAction(m.action, m.cost),
                          t - m_lastGScore));
                    }
                  }
                }
            }else {

              const auto& sis_s = safeIntervals(m_env.getLocation(s.state));
          	  Cost start_t = m_lastGScore;
      		  Cost end_t = sis_s.at(s.interval).end;

      		  State state_re = s.state;
      		  bool is_up_edge_collision = true;
      		  bool is_down_edge_collision = true;
      		  std::vector<startTime> re_start;
      		  flag_solution = false;

//      		  std::cout << "Current State " << s.state.x << "  " << s.state.y <<" Gscore" << m_lastGScore << " interval " << s.interval << " Direction " << s.dir << "  ********************************\n";
      		  jps_successors.clear();
      		  Cost wait_time_l = 0, wait_time_r = 0, wait_time_u = 0, wait_time_d = 0;
         	  if(s.dir & 0x1){
         		  s_temp.action = Action::Left;
         		  s_temp.dir = 0x01;
         		  getJPSSuccessors(s_temp, 0x01, 0, 0);
         	   }
         	   if(s.dir & 0x2){
         	   	  s_temp.action = Action::Right;
         	   	  s_temp.dir = 0x02;
       	   		  getJPSSuccessors(s_temp, 0x02, 0, 0);
         	   }
         	   if(s.dir & 0x4){
         	   	  s_temp.action = Action::Up;
         	   	  s_temp.dir = 0x04;
         	   	  getJPSSuccessors(s_temp, 0x04, 0, 0);
         	   }

         	   if(s.dir & 0x8){
         	   	  s_temp.action = Action::Down;
         	   	  s_temp.dir = 0x08;
           	   	  getJPSSuccessors(s_temp, 0x08, 0, 0);
         	   }

         	   //Generate the waiting successor
         	   Cost up_start_t = -1, down_start_t = -1, left_start_t = -1, right_start_t = -1;
         	   re_start.clear();
         	   if(m_env.stateValid(State(s.state.x, s.state.y + 1))
         			   && isTemporalObstacleAfterT(State(s.state.x, s.state.y + 1), m_lastGScore + 1, up_start_t)){
         		   JPSSIPPState temp_state = s;
         		   temp_state.flag_wait = false;
         		   temp_state.state.x = s.state.x;
         		   temp_state.state.y = s.state.y + 1;

        		   const auto& sis = safeIntervals(m_env.getLocation(State(s.state.x, s.state.y + 1)));
         		   Cost up_1 = -1;
         		   for (size_t i = 0; i < sis.size(); ++i) {
         			   const interval& si = sis[i];
         			   if (si.start - 1 > end_t) {
         				   break;
         			   }
         			   if( si.end < start_t + 1){
         				   continue;
         			   }
         			  up_1 = std::max<int>(start_t + 1, si.start);
           			  if(up_1 == m_lastGScore + 1) continue;

         			   if(!IsEdgeCollisions(State(s.state.x, s.state.y + 1), edgeCollision(up_1 - 1, Action::Down))){
             			   temp_state.interval = i;
                		   temp_state.dir = 0x00;
             			   for(size_t ii = 0; ii < sis_s.size(); ++ii){
             				   if(sis_s[ii].start == up_1 + 1
             						   && !IsEdgeCollisions(State(s.state.x, s.state.y), edgeCollision(up_1, Action::Up))){
             					   temp_state.dir |= 0x0b;
             					   break;
             				   }
             			   }
         				   temp_state.dir |= 0x07;
            			   jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Up,
         						   up_1 - m_lastGScore));
         			   }
         		   }

/*        		   if(up_start_t != -1  && up_start_t <= end_t
     	    				  && !IsEdgeCollisions(State(s.state.x, s.state.y + 1),edgeCollision(up_start_t,Action::Down))){
     	    			  re_start.push_back(startTime(up_start_t, Action::Up, 0x04, true));
         		   }
*/         	   }

     	      if(m_env.stateValid(State(s.state.x, s.state.y - 1)) &&
     	    		  isTemporalObstacleAfterT(State(s.state.x, s.state.y - 1), m_lastGScore + 1, down_start_t)){

        		   JPSSIPPState temp_state = s;
        		   temp_state.dir = 0x00;
        		   temp_state.flag_wait = false;
        		   temp_state.state.x = s.state.x;
        		   temp_state.state.y = s.state.y - 1;

        		   const auto& sis = safeIntervals(m_env.getLocation(State(s.state.x, s.state.y - 1)));
        		   Cost down_1 = -1;
        		   for (size_t i = 0; i < sis.size(); ++i) {
        			   const interval& si = sis[i];
        			   if (si.start - 1 > end_t) {
        				   break;
        			   }
        			   if( si.end < start_t + 1){
        				   continue;
        			   }
        			   down_1 = std::max<int>(start_t + 1, si.start);
        			   if(down_1 == m_lastGScore + 1) continue;

        			   if(!IsEdgeCollisions(State(s.state.x, s.state.y - 1), edgeCollision(down_1 - 1, Action::Up))){
            			   temp_state.interval = i;
            			   temp_state.dir = 0x00;
             			   for(size_t ii = 0; ii < sis_s.size(); ++ii){
             				   if(sis_s[ii].start == down_1 + 1 &&
             						   !IsEdgeCollisions(State(s.state.x, s.state.y), edgeCollision(down_1, Action::Down))){
             					   temp_state.dir |= 0x07;
             					   break;
             				   }
             			   }
        				   temp_state.dir |= 0x0b;
        				   jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Down,
        						   down_1 - m_lastGScore));
        			   }
        		   }

/*     	    	  if(down_start_t != -1  && down_start_t <= end_t
     	    			 && !IsEdgeCollisions(State(s.state.x, s.state.y - 1),edgeCollision(down_start_t,Action::Up))){
     	    		  re_start.push_back(startTime(down_start_t, Action::Down, 0x08, true));
     	    	  }
*/     	      }

     	      if(m_env.stateValid(State(s.state.x - 1, s.state.y)) &&
     	    		  isTemporalObstacleAfterT(State(s.state.x - 1, s.state.y), m_lastGScore + 1, left_start_t)){
       		   JPSSIPPState temp_state = s;
       		   temp_state.dir = 0x00;
       		   temp_state.flag_wait = false;
       		   temp_state.state.x = s.state.x - 1;
       		   temp_state.state.y = s.state.y;

       		   const auto& sis = safeIntervals(m_env.getLocation(State(s.state.x - 1, s.state.y)));
       		   Cost left_1 = -1;
       		   for (size_t i = 0; i < sis.size(); ++i) {
       			   const interval& si = sis[i];
       			   if (si.start - 1 > end_t) {
       				   break;
       			   }
       			   if( si.end < start_t + 1){
       				   continue;
       			   }
       			   left_1 = std::max<int>(start_t + 1, si.start);
       			   if(left_1 == m_lastGScore + 1) continue;

       			   temp_state.interval = i;
       			   temp_state.dir = 0x00;
       			   if(!IsEdgeCollisions(State(s.state.x - 1, s.state.y), edgeCollision(left_1 - 1, Action::Right))){
       				   temp_state.dir |= 0x01;
       			   }else break;
       			   for(size_t ii = 0; ii < sis_s.size(); ++ii){
        			   if(sis_s[ii].start == left_1 + 1 &&
        					   !IsEdgeCollisions(State(s.state.x, s.state.y), edgeCollision(left_1 -1, Action::Left))){
        				   temp_state.dir |= 0x02;
        				   break;
        			   }
        		   }

       			    Cost temp_t = left_1 + 1;
					if(m_env.stateValid(State(s.state.x - 1, s.state.y + 1))){ //Whether the successor need to restart the up direction
						if(m_env.isObstacle(State(s.state.x, s.state.y + 1))
								|| IsEdgeCollisions(State(s.state.x, s.state.y + 1), edgeCollision(left_1 -1, Action::Down))
								|| isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), left_1)
								|| isSafeAtT(State(s.state.x - 1, s.state.y + 1), temp_t)
								){
							if(!IsEdgeCollisions(State(s.state.x -1, s.state.y + 1), edgeCollision(left_1 - 1, Action::Down))){
								temp_state.dir |= 0x04;
							}
						}
/*						if(isSafeAtT(State(s.state.x - 1, s.state.y + 1), temp_t)){
							bool is_up_p = IsEdgeCollisions(State(s.state.x, s.state.y + 1), edgeCollision(left_1 - 1, Action::Down));
							bool is_up_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y + 1), edgeCollision(left_1, Action::Right));
							if(!(m_env.stateValid(State(s.state.x, s.state.y + 1)) && !is_up_p && !is_up_r
									&& isSafeAtT(State(s.state.x, s.state.y + 1), left_1)
									&& isTemporalObstacleAtT(State(s.state.x - 1, s.state.y + 1), left_1)
									&& isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), left_1 - 1))){
								if(!IsEdgeCollisions(State(s.state.x -1, s.state.y + 1), edgeCollision(left_1 - 1, Action::Down))){
									temp_state.dir |= 0x04;
								}
							}
						}*/
					}

					if(m_env.stateValid(State(s.state.x - 1, s.state.y - 1))){
						if(m_env.isObstacle(State(s.state.x, s.state.y - 1))
								|| IsEdgeCollisions(State(s.state.x, s.state.y - 1), edgeCollision(left_1 -1, Action::Up))
								|| isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), left_1)
								|| isSafeAtT(State(s.state.x - 1, s.state.y - 1), temp_t)
								){
							if(!IsEdgeCollisions(State(s.state.x - 1, s.state.y - 1), edgeCollision(left_1 - 1, Action::Up))){
								temp_state.dir |= 0x08;
							}
						}

/*						if(isSafeAtT(State(s.state.x - 1, s.state.y - 1), temp_t)){
							bool is_up_p = IsEdgeCollisions(State(s.state.x, s.state.y - 1), edgeCollision(left_1 - 1, Action::Up));
							bool is_up_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y - 1), edgeCollision(left_1, Action::Right));
							if(!(m_env.stateValid(State(s.state.x, s.state.y - 1)) && !is_up_p && !is_up_r && left_1 - 1 <= end_t && left_1 - 1 >= start_t
									&& isSafeAtT(State(s.state.x, s.state.y - 1), left_1)
									&& isTemporalObstacleAtT(State(s.state.x - 1, s.state.y - 1), left_1)
									&& isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), left_1 - 1))){
								if(!IsEdgeCollisions(State(s.state.x -1, s.state.y - 1), edgeCollision(left_1 - 1, Action::Up))){
									temp_state.dir |= 0x08;
								}
							}
						}*/
					}

       				jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Down,
       					   left_1 - m_lastGScore));

       		   }

/*     	    	  if(left_start_t != -1  && left_start_t <= end_t
     	    			  && !IsEdgeCollisions(State(s.state.x - 1, s.state.y),edgeCollision(left_start_t, Action::Right))){
     	    			  re_start.push_back(startTime(left_start_t, Action::Left, 0x01, true));
     	    	  }
*/     	      }

     	      if(isTemporalObstacleAfterT(State(s.state.x + 1, s.state.y), m_lastGScore + 1, right_start_t)){

          		   JPSSIPPState temp_state = s;
          		   temp_state.dir = 0x00;
          		   temp_state.flag_wait = false;
          		   temp_state.state.x = s.state.x + 1;
          		   temp_state.state.y = s.state.y;

          		   const auto& sis = safeIntervals(m_env.getLocation(State(s.state.x + 1, s.state.y)));
          		   Cost right_1 = -1;
          		   for (size_t i = 0; i < sis.size(); ++i) {
          			   const interval& si = sis[i];
          			   if (si.start - 1 > end_t) {
          				   break;
          			   }
          			   if( si.end < start_t + 1){
          				   continue;
          			   }
          			   right_1 = std::max<int>(start_t + 1, si.start);
          			   if(right_1 == m_lastGScore + 1) continue;

          			   temp_state.interval = i;
          			   temp_state.dir = 0x00;
          			   if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y), edgeCollision(right_1 - 1, Action::Left))){
          				   temp_state.dir |= 0x02;
          			   }else break;
   //       			       std::cout << " State " << s.state.x << " " << s.state.y << " Time : " << start_t << " " << si.start - 1  << " Curr " << m_lastGScore << " \n";
          			   for(size_t ii = 0; ii < sis_s.size(); ++ii){
          				   if(sis_s[ii].start == right_1 + 1 &&
           					   !IsEdgeCollisions(State(s.state.x, s.state.y), edgeCollision(right_1 -1, Action::Right))){
          					   temp_state.dir |= 0x01;
          				   }
          			   }
          			   Cost temp_t = right_1 + 1;
//          			   std::cout << "State : " << s.state.x << " " << s.state.y+1 << " " << right_1 << "\n";
          			   if(m_env.stateValid(State(s.state.x + 1, s.state.y + 1))){ //Whether the successor need to restart the up direction
          				   if(m_env.isObstacle(State(s.state.x, s.state.y + 1))
   								|| IsEdgeCollisions(State(s.state.x, s.state.y + 1), edgeCollision(right_1 - 1, Action::Down))
   								|| isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), right_1)
//								|| isSafeAtT(State(s.state.x + 1, s.state.y + 1), temp_t)
								){
          					   if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(right_1 - 1, Action::Down))){
          						   temp_state.dir |= 0x04;
          					   	}
          				   }
          				   if(isSafeAtT(State(s.state.x + 1, s.state.y + 1), temp_t)){
          					   bool is_up_p = IsEdgeCollisions(State(s.state.x, s.state.y + 1), edgeCollision(right_1 - 1, Action::Down));
          					   bool is_up_r = IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(right_1, Action::Left));
          					   if(!(m_env.stateValid(State(s.state.x, s.state.y + 1)) && !is_up_p && !is_up_r
          							   && isSafeAtT(State(s.state.x, s.state.y + 1), right_1)
   										&& isTemporalObstacleAtT(State(s.state.x + 1, s.state.y + 1), right_1)
										&& isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), right_1 - 1))){
          						   if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(right_1 - 1, Action::Down))){
          							   temp_state.dir |= 0x04;
          						   }
          					   }
          				   }
          			   }
          			   if(m_env.stateValid(State(s.state.x + 1, s.state.y - 1))){
          				   if(m_env.isObstacle(State(s.state.x, s.state.y - 1))
          						   || IsEdgeCollisions(State(s.state.x, s.state.y - 1), edgeCollision(right_1 -1, Action::Up))
								   || isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), right_1)
								   //|| isSafeAtT(State(s.state.x + 1, s.state.y - 1), temp_t)
								   ){
          					   if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(right_1 - 1, Action::Up))){
          						   temp_state.dir |= 0x08;
          					   }
          				   }

          				   if(isSafeAtT(State(s.state.x + 1, s.state.y - 1), temp_t)){
          					   bool is_up_p = IsEdgeCollisions(State(s.state.x, s.state.y - 1), edgeCollision(right_1 - 1, Action::Up));
          					   bool is_up_r = IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(right_1, Action::Left));
          					   if(!(m_env.stateValid(State(s.state.x, s.state.y - 1)) && !is_up_p && !is_up_r
          							   && isSafeAtT(State(s.state.x, s.state.y - 1), right_1)
   										&& isTemporalObstacleAtT(State(s.state.x + 1, s.state.y - 1), right_1)
										&& isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), right_1 - 1))){
          						   if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(right_1 - 1, Action::Up))){
          							   temp_state.dir |= 0x08;
          						   }
          					   }
          				   }
          			   }
          			   jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Down,
          					   right_1 - m_lastGScore));
          		   }
/*    	    	  if(right_start_t != -1  && right_start_t <= end_t
     	    			  && !IsEdgeCollisions(State(s.state.x + 1, s.state.y),edgeCollision(right_start_t,Action::Left))){
     	    			  re_start.push_back(startTime(right_start_t, Action::Left, 0x02, true));
     	    	  }
*/     	      }

/*     	      std::sort(re_start.begin(), re_start.end());
     	      JPSSIPPState temp_state = s;
     	      temp_state.dir = 0x0;
     	      for(int re_i = 0; re_i < re_start.size(); re_i++){
					if(re_start[re_i].rt == -1) continue;
					temp_state.action = Action::Left;
					temp_state.dir |= re_start[re_i].dir;
					temp_state.flag_wait = re_start[re_i].flag_wait;
					int re_ii;
					for(re_ii = re_i + 1; re_ii < re_start.size(); re_ii++){
						if(re_start[re_ii].rt == re_start[re_i].rt){
							temp_state.dir |= re_start[re_ii].dir;
							if(re_start[re_ii].flag_wait) temp_state.flag_wait = re_start[re_ii].flag_wait;
						}else break;
					}
					if(re_start[re_i].rt != m_lastGScore)
					jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Left,
							re_start[re_i].rt - m_lastGScore));

					m_env.num_generation = m_env.num_generation - (re_start.size() -re_ii);
					break;
     	      }*/

          	  for ( auto& m : jps_successors) {

          		  neighbors.emplace_back(Neighbor<JPSSIPPState, JPSSIPPAction, Cost>(
  						  JPSSIPPState(m.state.state, m.state.interval, m.state.dir,  m_lastGScore + m.cost, m.state.flag_wait),
						  	  JPSSIPPAction(m.action, m.cost), m.cost));
// 				  	  std::cout << "Successor-: " << m.state.state.x << " "<< m.state.state.y << " Cost ++" << m.cost + m_lastGScore << " flag " << m.state.flag_wait  << " dir " << m.state.dir << "\n";
  				 m_env.num_generation++;
          	  }
          }
       }

       void getJPSSuccessors(JPSSIPPState s, unsigned int dir, Cost current_cost, int depth){

             	if (isSolution(s)) {
           			flag_solution = true;
           			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(s, Action::Left, current_cost));
           			return ;
             	}
             	if(current_cost >= 5){
             		jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(s, Action::Left, current_cost));
             		return;
             	}

         		const auto& si_s = safeIntervals(m_env.getLocation(s.state));
         		Cost start_t = m_lastGScore + current_cost;
         		Cost end_t = si_s.at(s.interval).end;

                JPSSIPPState current_successor = s;
                current_successor.dir = 0x0;
                std::vector<startTime> re_start_s;
                Cost re_ac_l = -1, re_ac_r = -1, re_ac_u = -1, re_ac_d = -1; //The current state s may be the waiting jump point. This values are used to
                															 // record the re-start time for different direction.
                															 // The s with min(re_ac_l, re_ac_r, re_ac_d, re_ac_u) is the next waiting jump point.

                if(depth != 0) m_env.num_generation++;

         		State state_up(s.state.x, s.state.y + 1);
         		State state_down(s.state.x, s.state.y -1);
         		bool is_up_valid = m_env.stateValid(state_up);
         		bool is_down_valid = m_env.stateValid(state_down);
         		bool is_up_edge_collision = true;
         		bool is_down_edge_collision = true;

         		Cost successor_start = -1, successor_end = -1; //Record the safe interval of the current successor that s can go though.
				Cost successor_next_start = -1, successor_next_end = -1; //Record the next safe interval of current successor.

				size_t successor_interval_u = -1;
				size_t successor_interval_d = -1;
         		bool is_up_temp = false;
         		Cost successor_start_u = -1; //for the up direction, the start time of safe interval that s can go though,
         									 //which will be used in the up direction and horizontal direction
         		Cost successor_end_u = -1;
         		Cost successor_next_start_u = -1; //for the up direction, the next safe inteval of the up successor
         										  //which will be used in the up direction
         		Cost successor_next_end_u = -1;

         		Cost successor_start_d = -1; //for the down direction, the start time of safe interval that s can go though
         		Cost successor_end_d = -1; //for the down direction, the start time of safe interval that s can go though
         		Cost successor_next_start_d = -1; //for the down direction, the start time of safe interval that s can go though
         		Cost successor_next_end_d = -1; //for the down direction, the start time of safe interval that s can go though

         		if(is_up_valid){
         			is_up_edge_collision =  IsEdgeCollisions(state_up, edgeCollision(m_lastGScore + current_cost, Action::Down)); //for the up, check the edge collision
         			if(!is_up_edge_collision) {
        				findSafeInterval(state_up, start_t + 1, successor_interval_u,
        				          					   successor_start_u, successor_end_u, successor_next_start_u, successor_next_end_u);
         			}
        		}
         		if(is_down_valid){
         			is_down_edge_collision =  IsEdgeCollisions(state_down,edgeCollision(m_lastGScore + current_cost,Action::Up));
         			if(!is_down_edge_collision){
           				findSafeInterval(state_down, start_t + 1, successor_interval_d,
            				          					 successor_start_d, successor_end_d, successor_next_start_d, successor_next_end_d);
         			}
         		}

             	if((dir & 0x01) && !flag_solution){ // the left direction
            		current_successor.state.x = s.state.x - 1;
            		current_successor.state.y = s.state.y;

            		 if(m_env.stateValid(current_successor.state) &&
            					!IsEdgeCollisions(current_successor.state,edgeCollision(m_lastGScore + current_cost, Action::Right))){
            				size_t successor_interval;
            				successor_start = -1; successor_end = -1;
            				successor_next_start = -1; successor_next_end = -1;
            				findSafeInterval(current_successor.state, start_t + 1, successor_interval,                  //find the safe interval
            				          						successor_start, successor_end, successor_next_start, successor_next_end);

            				if(successor_next_start != -1 && depth != 0){ // the current s can be the waiting jump point on the left direction.
            					if( end_t >= successor_next_start - 1 && successor_next_start - 1 != m_lastGScore + current_cost){
            							if(!IsEdgeCollisions(current_successor.state,edgeCollision(successor_next_start - 1,Action::Right))){
//            								JPSSIPPState temp_state = s;
//            								temp_state.dir = 0x1;
//            								temp_state.flag_wait = true;
//            								temp_state.action = Action::Left;
//             								jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Left,
//             									next_start - 1 - m_lastGScore));
            								re_ac_l = successor_next_start - 1;
//            								re_start_s.push_back(startTime(successor_next_start - 1, Action::Left, 0x01, true));
            							}
            					}
            				}

            			std::vector<startTime> re_start;
           				if(successor_start !=- 1 && re_ac_l != m_lastGScore + current_cost + 1){//
            					current_successor.interval = successor_interval;
            					current_successor.dir = 0x00;
                				Cost up_start_t = -1, down_start_t = -1, right_start_t = -1, left_start_t = -1;
                				if(m_env.isJumpPoint(current_successor.state)){ //The successor may be a jump point.
                					if(m_env.stateValid(State(s.state.x - 1, s.state.y + 1))){ //Whether the successor need to restart the up direction
                						if(m_env.isObstacle(state_up)
                								|| is_up_edge_collision
												|| isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), m_lastGScore + current_cost + 1)){
                							// at the time m_lastGScore + current_cost + 1 successor need to restart the up direction
                							if(!IsEdgeCollisions(State(s.state.x -1, s.state.y + 1),
                									edgeCollision(m_lastGScore + current_cost + 1, Action::Down))){
                								up_start_t = m_lastGScore + current_cost + 1;
                								re_start.push_back(startTime(up_start_t, Action::Up, 0x04, false));
                							}
                						} else {
                							// after the time m_lastGScore + current_cost + 1 successor need to restart the up direction
                							isTemporalObstacleAfterT(State(s.state.x - 1, s.state.y + 1), m_lastGScore + current_cost + 1, up_start_t);
                							Cost temp_start_t = -1, temp_end_t = -1;
                							size_t iv = -1;
                							bool is_up_p = false, is_up_r = false;
                							if(up_start_t == m_lastGScore + current_cost + 1){
                								temp_start_t = successor_start_u;
                								is_up_p = is_up_edge_collision;
                								is_up_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y + 1),
                											edgeCollision(m_lastGScore + current_cost + 1,Action::Right));
                							} else if(up_start_t - 1 >=start_t && up_start_t - 1 <= end_t){
                								findSafeInterval(state_up, up_start_t, iv, temp_start_t, temp_end_t);
                		           				is_up_p = IsEdgeCollisions(state_up,edgeCollision(up_start_t - 1, Action::Down));
                		           				is_up_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y + 1), edgeCollision(up_start_t, Action::Right));
                							}
                 							if(up_start_t != -1 && up_start_t <= successor_end){
                 								//if it is not the case like the exmaple A.....X, and up_start_t is in the current safe interval.
                 								//                                       B.....Y
                 									while(1){
                 										if(((is_up_valid && temp_start_t != -1 && !is_up_p && !is_up_r)
                 												&& isTemporalObstacleAtT(State(s.state.x - 1, s.state.y + 1), up_start_t)
																&& isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), up_start_t - 1))){
                 											Cost temp = up_start_t + 1;
                 											isTemporalObstacleAfterT(State(s.state.x - 1, s.state.y + 1), temp, up_start_t);

                 											if(up_start_t != -1 && up_start_t <= successor_end
                 													&&!IsEdgeCollisions(State(s.state.x - 1, s.state.y + 1), edgeCollision(up_start_t, Action::Down))){
                                    							if(up_start_t - 1 >= start_t && up_start_t - 1 <= end_t){
                                    								findSafeInterval(state_up, up_start_t, iv, temp_start_t, temp_end_t);
                                    		           				is_up_p = IsEdgeCollisions(state_up,edgeCollision(up_start_t - 1, Action::Down));
                                    		           				is_up_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y + 1), edgeCollision(up_start_t, Action::Right));
                                    		           				continue;
                                    							} else{
                                    								re_start.push_back(startTime(up_start_t, Action::Up, 0x04, true));
                                    								break;
                                    							}
                 											} else break;
                 										} else {
                 											if(!IsEdgeCollisions(State(s.state.x - 1, s.state.y + 1), edgeCollision(up_start_t, Action::Down)))
                 											re_start.push_back(startTime(up_start_t, Action::Up, 0x04, true));
                 											break;
                 										}
                 									}
                            				}
                						}
                					}
                					if(m_env.stateValid(State(s.state.x - 1, s.state.y - 1))){ //Whether the successor need to restart the down direction
                						if(m_env.isObstacle(state_down)
                								|| is_down_edge_collision
												|| isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), m_lastGScore + current_cost + 1)){
                							if(!IsEdgeCollisions(State(s.state.x - 1, s.state.y - 1), edgeCollision(m_lastGScore + current_cost + 1, Action::Up))){
                	                    					down_start_t = m_lastGScore + current_cost + 1;
                	                    					re_start.push_back(startTime(down_start_t, Action::Down, 0x08, false));
                	                		}
                						} else {
                							isTemporalObstacleAfterT(State(s.state.x - 1, s.state.y - 1), m_lastGScore + current_cost + 1, down_start_t);
                							Cost temp_start_t = -1, temp_end_t = -1;
                							size_t in = -1;
                							bool is_down_p = false, is_down_r = false;
                							if(down_start_t == m_lastGScore + current_cost + 1){
                								temp_start_t = successor_start_d;
                								is_down_p = is_down_edge_collision;
                								is_down_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y - 1),
                								            	edgeCollision(m_lastGScore + current_cost + 1,Action::Right));

                							} else if(down_start_t - 1 >= start_t && down_start_t - 1 <= end_t){
                		           				findSafeInterval(state_down, down_start_t, in, temp_start_t, temp_end_t);
                		           				is_down_p = IsEdgeCollisions(state_down,edgeCollision(down_start_t - 2,Action::Up));
                		           				is_down_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y - 1), edgeCollision(down_start_t, Action::Right));
                							}
                							if(down_start_t != -1 && down_start_t <= successor_end){
                								while(1){
                									if(((is_down_valid && temp_start_t != -1 && !is_down_p && !is_down_r)
                											&& isTemporalObstacleAtT(State(s.state.x - 1, s.state.y - 1), down_start_t)
															&& isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), down_start_t - 1))){
                										Cost temp = down_start_t + 1;
                										isTemporalObstacleAfterT(State(s.state.x - 1, s.state.y - 1), temp, down_start_t);
                										if(down_start_t != -1 && down_start_t <= successor_end
                												&& !IsEdgeCollisions(State(s.state.x - 1, s.state.y -1), edgeCollision(down_start_t, Action::Up))){
                											if(down_start_t - 1 >= start_t && down_start_t -1 <= end_t){
                												findSafeInterval(state_down, down_start_t, in, temp_start_t, temp_end_t);
                                		           				is_down_p = IsEdgeCollisions(state_down,edgeCollision(down_start_t - 1,Action::Up));
                                		           				is_down_r = IsEdgeCollisions(State(s.state.x - 1, s.state.y - 1), edgeCollision(down_start_t, Action::Right));
                                		           				continue;
                											} else {
                												re_start.push_back(startTime(down_start_t, Action::Down, 0x08, true));
                												break;
                											}
                										} else break;
                									} else {
                										if(!IsEdgeCollisions(State(s.state.x - 1, s.state.y -1), edgeCollision(down_start_t, Action::Up)))
                										re_start.push_back(startTime(down_start_t, Action::Down, 0x08, true));
                										break;
                									}
                								}
                							}
                						}
                					}
                				}
                				Cost next_start_s = -1;
                				bool is_right = false;
            					if(si_s.size() > s.interval + 1){ // Whether the successor need to go back, that is, restart the right direction.
            						next_start_s = si_s.at(s.interval + 1).start;
            						if(!IsEdgeCollisions(s.state,edgeCollision(next_start_s - 1, Action::Left))
            								&& successor_end >= next_start_s - 1){
            							is_right = true;
            							right_start_t = next_start_s - 1;
            							re_start.push_back(startTime(right_start_t, Action::Right, 0x02, true));
            						}
            					 }

            					std::sort(re_start.begin(), re_start.end()); //For the re-start direction, choose the minimum re-start time.
            					Cost re_ac = -1;
            					unsigned int current_dir = 0x00;
            					for(int re_i = 0; re_i < re_start.size(); re_i++){
            						if(re_start[re_i].rt == -1) continue;
            						current_successor.action = Action::Left;
            						current_dir = re_start[re_i].dir;
            						for(int re_ii = re_i + 1; re_ii < re_start.size(); re_ii++){
            							if(re_start[re_ii].rt == re_start[re_i].rt){
            								current_dir |= re_start[re_ii].dir;
            							}else break;
            						}
            						re_ac = re_start[re_i].rt;
            						if(re_ac == m_lastGScore + current_cost + 1){
            							current_dir |= 0x01;
            						}
            						break;
            					}

            					if(re_ac == -1){ // There is no re-start direction.

            						if(isTemporalObstacleAfterT(State(s.state.x - 2, s.state.y), m_lastGScore + current_cost + 1)){
            							current_successor.dir = 0x01;
            		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
            		          					Action::Left, current_cost + 1));
            						} else{
                						current_successor.dir = 0x01;
                						getJPSSuccessors(current_successor, 0x01, current_cost + 1, depth + 1);
            						}
            					} else {
            					/*	if(re_ac == m_lastGScore + current_cost + 1) { // set the re-start direction.
            							current_successor.dir = current_dir;
            							if(current_successor.dir !=  0x00){
            								current_successor.flag_wait = false;
                		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
                		          					Action::Left, re_ac - m_lastGScore));
            							}
            						} else {
            							current_successor.dir = 0x01;
            							getJPSSuccessors(current_successor, 0x01, current_cost + 1, depth + 1);

            							current_successor.dir = current_dir;
            							current_successor.dir = current_successor.dir & 0xe;
            							if(current_successor.dir !=  0x00){
            								current_successor.flag_wait = false;
                		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
                		          					Action::Left, re_ac - m_lastGScore));
            							}
            						}*/

            						if(re_ac == m_lastGScore + current_cost + 1){
            							current_successor.dir = current_dir;
            						} else current_successor.dir = 0x01;

            		          		if(!m_env.stateValid(State(current_successor.state.x - 1, current_successor.state.y)) ||
            		          					IsEdgeCollisions(State(current_successor.state.x - 1, current_successor.state.y),
            		          							edgeCollision(m_lastGScore + current_cost + 2, Action::Right))){ // the left direction is blocked.
            		          				current_successor.dir = current_successor.dir & 0xe;
            		          		}
            		          		if(isSolution(current_successor)) flag_solution = true;
            		          		if(current_successor.dir != 0x0 || m_env.isSolution(current_successor.state)){
            		          			current_successor.flag_wait = false;
            		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
            		          					Action::Left, current_cost + 1));
            		          		} else if(re_ac != -1){
            		          			current_successor.dir = current_dir;
            		          			current_successor.flag_wait = false;
            		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
            		          					Action::Left, re_ac - m_lastGScore));
            		          		}
            					}
           				}
            		}
             	}

             	if((dir& 0x02) && !flag_solution){
            			current_successor.state.x = s.state.x + 1;
            			current_successor.state.y = s.state.y;
            			if(m_env.stateValid(current_successor.state)
            					&& !IsEdgeCollisions(current_successor.state,edgeCollision(m_lastGScore + current_cost,Action::Left))){
            				size_t successor_interval;
            				successor_start = -1; successor_end = -1;
            				successor_next_start = -1; successor_next_end = -1;
            				findSafeInterval(current_successor.state, start_t + 1, successor_interval,
            						successor_start, successor_end, successor_next_start, successor_next_end);
            				bool flag_push = false;

            				if(successor_next_start != -1 && depth != 0){
            					if( end_t >= successor_next_start - 1 && successor_next_start - 1 != m_lastGScore + current_cost){
            						if(!IsEdgeCollisions(current_successor.state,edgeCollision(successor_next_start - 1,Action::Left))){
            							re_ac_r = successor_next_start -1;
            							re_start_s.push_back(startTime(successor_next_start - 1, Action::Right, 0x02, true));
            						}
            					}
            				}
            				if(successor_start != -1 && re_ac_r != m_lastGScore + current_cost + 1){
            					current_successor.interval = successor_interval;
            					current_successor.dir = 0x00;

            					Cost up_start_t = -1, down_start_t = -1, left_start_t = -1, right_start_t = -1;
            					std::vector<startTime> re_start;
                				if(m_env.isJumpPoint(current_successor.state)){

                					if(m_env.stateValid(State(s.state.x + 1, s.state.y + 1))){
                						if(m_env.isObstacle(state_up)
                								|| is_up_edge_collision
												|| isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), m_lastGScore + current_cost + 1)){
                							if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(m_lastGScore + current_cost, Action::Down))){
                								up_start_t = m_lastGScore + current_cost + 1;
                								re_start.push_back(startTime(up_start_t, Action::Up, 0x04, false));
                							}
                						} else{
                							isTemporalObstacleAfterT(State(s.state.x + 1, s.state.y + 1), m_lastGScore + current_cost + 1, up_start_t);
                							Cost temp_start_t = -1, temp_end_t = -1;
                							size_t iv = -1;
                							bool is_up_p = false, is_up_l = false;
                							if(up_start_t == m_lastGScore + current_cost + 1){
                								temp_start_t = successor_start_u;
                								is_up_p = is_up_edge_collision;
                								is_up_l = IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1),
                								         	 edgeCollision(m_lastGScore + current_cost + 1,Action::Left));
                							} else{
                								if(up_start_t - 1 >= start_t && up_start_t - 1 <= end_t){
                									findSafeInterval(state_up, up_start_t, iv, temp_start_t, temp_end_t);
                									is_up_p = IsEdgeCollisions(state_up,edgeCollision(up_start_t - 1, Action::Down));
                									is_up_l = IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(up_start_t, Action::Left));
                								}
                							}

                 							if(up_start_t != -1 && up_start_t <= successor_end){
                 								while(1){
                 									if(is_up_valid && temp_start_t != -1 && !is_up_p && !is_up_l
                 											&& isTemporalObstacleAtT(State(s.state.x + 1, s.state.y + 1), up_start_t)
                 											&& isTemporalObstacleAtT(State(s.state.x, s.state.y + 1), up_start_t - 1)){
                 										Cost temp = up_start_t + 1;
                 										isTemporalObstacleAfterT(State(s.state.x + 1, s.state.y + 1), up_start_t + 1, up_start_t);
                 										if(up_start_t <= successor_end
                 												&& !IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(up_start_t, Action::Down))){
                            								if(up_start_t - 1 >=start_t && up_start_t - 1 <= end_t){
                            									findSafeInterval(state_up, up_start_t, iv, temp_start_t, temp_end_t);
                            									is_up_p = IsEdgeCollisions(state_up,edgeCollision(up_start_t - 1, Action::Down));
                            									is_up_l = IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(up_start_t, Action::Left));
                            									continue;
                            								} else {
                            									re_start.push_back(startTime(up_start_t, Action::Up, 0x04, true));
                            									break;
                            								}
                 										} else break;

                 									}else {
                 										if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y + 1), edgeCollision(up_start_t, Action::Down)))
                 										re_start.push_back(startTime(up_start_t, Action::Up, 0x04, true));
                 										break;
                 									}
                 								}
                            				}
                						}
                					}

                					if(m_env.stateValid(State(s.state.x + 1, s.state.y - 1))){
                						if(m_env.isObstacle(state_down)
                								|| is_down_edge_collision
												|| isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), m_lastGScore + current_cost + 1)){
                							if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(m_lastGScore + current_cost, Action::Up))){
                								down_start_t = m_lastGScore + current_cost + 1;
                								re_start.push_back(startTime(down_start_t, Action::Down, 0x08, false));
                							}
                						} else{
                							isTemporalObstacleAfterT(State(s.state.x + 1, s.state.y - 1), m_lastGScore + current_cost + 1, down_start_t);
                							Cost temp_start_t = -1, temp_end_t = -1;
                							size_t iv = -1;
                							bool is_down_p = false, is_down_l = false;
                							if(down_start_t == m_lastGScore + current_cost + 1){
                								temp_start_t = successor_start_d;
                								is_down_p = is_down_edge_collision;
                								is_down_l = IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1),
                								         		edgeCollision(m_lastGScore + current_cost + 1,Action::Left));
                							} else{
                								if(down_start_t - 1 >=start_t && down_start_t - 1 <= end_t){
                									findSafeInterval(state_down, down_start_t, iv, temp_start_t, temp_end_t);
                									is_down_p = IsEdgeCollisions(state_down,edgeCollision(down_start_t - 1,Action::Up));
                									is_down_l = IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(down_start_t, Action::Left));
                								}
                							}
                 							if(down_start_t != -1 && down_start_t <= successor_end){
                 								while(1){
                 									if(is_down_valid && temp_start_t != -1 && !is_down_p && !is_down_l
                 											&& isTemporalObstacleAtT(State(s.state.x + 1, s.state.y - 1), down_start_t)
                 											&& isTemporalObstacleAtT(State(s.state.x, s.state.y - 1), down_start_t - 1)){
                 										isTemporalObstacleAfterT(State(s.state.x + 1, s.state.y - 1), down_start_t + 1, down_start_t);
                 										if(down_start_t != -1 && down_start_t <= successor_end
                                							&& !IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(down_start_t, Action::Up))){
                            								if(down_start_t -1 >= start_t && down_start_t - 1 <= end_t){
                            									findSafeInterval(state_down, down_start_t, iv, temp_start_t, temp_end_t);
                            									is_down_p = IsEdgeCollisions(state_down,edgeCollision(down_start_t - 1, Action::Up));
                            									is_down_l = IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(down_start_t, Action::Left));
                            									continue;
                            								}else{
                            									re_start.push_back(startTime(down_start_t, Action::Down, 0x08, true));
                            									break;
                            								}
                            							} else break;

                 									} else {
                 										if(!IsEdgeCollisions(State(s.state.x + 1, s.state.y - 1), edgeCollision(down_start_t, Action::Up)))
                            									re_start.push_back(startTime(down_start_t, Action::Down, 0x08, true));
                 										break;
                 									}
                 								}
                            				}
                						}
                					}

                				}

                				Cost next_start_s = -1;
                				if(si_s.size()> s.interval + 1) {
                					next_start_s = si_s.at(s.interval + 1).start;
                					if(!IsEdgeCollisions(s.state, edgeCollision(next_start_s - 1, Action::Right))
                							&& successor_end >= next_start_s - 1){
                						left_start_t = next_start_s - 1;
                						re_start.push_back(startTime(left_start_t, Action::Left, 0x01, true));
                					}
                				}

            					std::sort(re_start.begin(), re_start.end());
            					Cost re_ac = -1;
            					unsigned int restart_dir = 0x00;
            					for(int re_i = 0; re_i < re_start.size(); re_i++){
            						if(re_start[re_i].rt == -1) continue;
            						current_successor.action = Action::Right;
            						restart_dir |= re_start[re_i].dir;
            						for(int re_ii = re_i + 1; re_ii < re_start.size(); re_ii++){
            							if(re_start[re_ii].rt == re_start[re_i].rt){
            								restart_dir |= re_start[re_ii].dir;
            							}else break;
            						}
            						re_ac = re_start[re_i].rt;
            						if(re_ac == m_lastGScore + current_cost + 1){
            							restart_dir |= 0x02;
            						}
            						break;
            					}
            					if(re_ac == -1){
//            						current_successor.dir = 0x02;
//            						getJPSSuccessors(current_successor, 0x02, current_cost + 1, depth + 1);

               						if(isTemporalObstacleAfterT(State(s.state.x + 2, s.state.y), m_lastGScore + current_cost + 1)){
                							current_successor.dir = 0x02;
                		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
                		          					Action::Left, current_cost + 1));
                					} else{
                    						current_successor.dir = 0x02;
                    						getJPSSuccessors(current_successor, 0x02, current_cost + 1, depth + 1);
                					}

            					}else{

/*            						if(re_ac == m_lastGScore + current_cost + 1) { // set the re-start direction.
            							current_successor.dir = restart_dir;
            							if(current_successor.dir !=  0x00){
            								current_successor.flag_wait = false;
                		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
                		          					Action::Right, re_ac - m_lastGScore));
            							}
            						} else {
            							current_successor.dir = 0x02;
            							getJPSSuccessors(current_successor, 0x02, current_cost + 1, depth + 1);
            							current_successor.dir = restart_dir;

            							isTemporalObstacleAfterT(State(s.state.x + 2, s.state.y), m_lastGScore + current_cost + 1, right_start_t);
            							if(left_start_t != re_ac) current_successor.dir = current_successor.dir & 0xd;

            							if(current_successor.dir !=  0x00){
            								current_successor.flag_wait = false;
                		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
                		          					Action::Left, re_ac - m_lastGScore));
            							}
            						}*/

            						if(re_ac == m_lastGScore + current_cost + 1){
            							current_successor.dir = restart_dir;
            						} else current_successor.dir = 0x02;

              		          		if(!m_env.stateValid(State(current_successor.state.x + 1, current_successor.state.y)) ||
                		          					IsEdgeCollisions(State(current_successor.state.x + 1, current_successor.state.y),
                		          							edgeCollision(m_lastGScore + current_cost + 2, Action::Left))){
                		          				current_successor.dir = current_successor.dir & 0xd;
                		          	}
              		          		if(isSolution(current_successor)) flag_solution = true;
               		          		if(current_successor.dir != 0x0 || m_env.isSolution(current_successor.state)){
               		          			current_successor.flag_wait = false;
               		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
               		          					Action::Left, current_cost + 1));
               		          		} else if(re_ac != -1){
               		          			current_successor.dir = restart_dir;
               		          			current_successor.flag_wait = false;
               		          			jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor,
               		          					Action::Left, re_ac - m_lastGScore));
               		          		}
            					}
            				}
            			}
             	}

            	if((dir & 0x04) && !flag_solution){
            		current_successor.state = state_up;
             		if(is_up_valid && !is_up_edge_collision){
            				size_t successor_interval =  successor_interval_u;
            				successor_start = successor_start_u; successor_end = successor_end_u;
            				successor_next_start = successor_next_start_u; successor_next_end = successor_next_end_u;
//            				findSafeInterval(current_successor.state, start_t + 1, successor_interval,
//            				          						successor_start, successor_end, successor_next_start, successor_next_end);

            				bool flag_push = false;
            				Cost re_ac_u = -1;
            				if(successor_next_start != -1 && depth != 0){
         						if(end_t >= successor_next_start - 1 && successor_next_start - 1 != m_lastGScore + current_cost){
         							if(!IsEdgeCollisions(current_successor.state,edgeCollision(successor_next_start - 1, Action::Down))){
//               						JPSSIPPState temp_state = s;
//               						temp_state.dir = 0x4;
//               						temp_state.interval = s.interval;
//               						temp_state.action = Action::Up;
//               						temp_state.flag_wait = true;
//             							jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Up, next_start -1 - m_lastGScore));
               							re_ac_u = successor_next_start - 1;
//               							re_start_s.push_back(startTime(successor_next_start - 1, Action::Up, 0x04, true));
               						}
           						}
            				}
            				Cost up_start_t = -1, down_start_t = -1;

            				if(successor_start != -1 && re_ac_u != m_lastGScore + current_cost + 1){
            					current_successor.interval = successor_interval;
            					current_successor.dir = 0x00;
            					Cost next_start_s = -1;
            					bool flag_re_down = false;
               					if(si_s.size()> s.interval + 1){ // check whether go back
               						next_start_s = si_s.at(s.interval + 1).start;
               						if(!IsEdgeCollisions(s.state, edgeCollision(next_start_s - 1, Action::Up)) && successor_end >= next_start_s - 1){
               							JPSSIPPState temp_state = current_successor;
               							temp_state.dir = 0x08;
               							temp_state.flag_wait = true;
               							down_start_t = next_start_s -1;
  //            							if(down_start_t != m_lastGScore + current_cost + 1)
  //            								jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Down, next_start_s -1 - m_lastGScore));
  //            							else flag_re_down = true;
               							flag_re_down = true;
               						}
               					 }
               					if(isTemporalObstacleAfterT(State(s.state.x, s.state.y + 2), m_lastGScore + current_cost + 1)){
               						flag_re_down  = true;
               					}
             					if(isTemporalObstacleAfterT(State(s.state.x, s.state.y + 2), m_lastGScore + current_cost + 1)
            							|| isTemporalObstacleAfterT(State(s.state.x - 1, s.state.y + 1), m_lastGScore + current_cost + 1)
             							|| isTemporalObstacleAfterT(State(s.state.x + 1, s.state.y + 1), m_lastGScore + current_cost + 1)
         							|| (m_env.is_limit && current_cost > m_env.limit_jump)){

         							if(down_start_t == m_lastGScore + current_cost + 1){
         								current_successor.dir = 0x0f;
         							} else {
         								current_successor.dir = 0x07;
         							}
                    				current_successor.flag_wait = false;
             						current_successor.action = Action::Up;
             						jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor, Action::Up, current_cost + 1));
             					} else {
                					if(flag_re_down){
                							if(down_start_t == m_lastGScore + current_cost + 1) current_successor.dir = 0x0f;
                							else current_successor.dir = 0x07;
                    						current_successor.flag_wait = false;
                    						if(isSolution(current_successor)) flag_solution = true;
                    						jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor, Action::Up, current_cost + 1));
                					}else{
             							current_successor.dir = 0x07;
             							getJPSSuccessors(current_successor, 0x07, current_cost + 1, depth + 1);
                					}
                				}
            				}
            			}
             	}
             	if((dir & 0x08) && !flag_solution){
             		current_successor.state = state_down;
          	 	 	if(is_down_valid && !is_down_edge_collision){
        				size_t successor_interval =  successor_interval_d;
        				successor_start = successor_start_d; successor_end = successor_end_d;
        				successor_next_start = successor_next_start_d; successor_next_end = successor_next_end_d;
//            				findSafeInterval(current_successor.state, start_t + 1, successor_interval,
//            				          						successor_start, successor_end, successor_next_start, successor_next_end);

            			    if(successor_next_start != -1 && depth != 0){
         						if( end_t >= successor_next_start - 1 && successor_next_start - 1 != m_lastGScore + current_cost){
         							if(!IsEdgeCollisions(current_successor.state,edgeCollision(successor_next_start - 1, Action::Up))){
//         								JPSSIPPState temp_state = s;
//         								temp_state.dir = 0x08;
//         								temp_state.interval = s.interval;
//         								temp_state.action = Action::Down;
//         								temp_state.flag_wait = true;
//            							jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Down, next_start - 1 - m_lastGScore));
              							re_ac_d = successor_next_start - 1;
//                						re_start_s.push_back(startTime(successor_next_start - 1, Action::Down, 0x08, true));
         							}
         						}
            				}

            			    Cost up_start_t = -1, down_start_t = -1;
            			    if(successor_start != -1 && re_ac_d !=  m_lastGScore + current_cost + 1){
            					current_successor.interval = successor_interval;
            					current_successor.dir = 0x00;
            					Cost next_start_s = -1;
            					bool flag_re_up = false;
               					if(si_s.size()> s.interval + 1){
               						next_start_s = si_s.at(s.interval + 1).start;
               						if(!IsEdgeCollisions(s.state, edgeCollision(next_start_s - 1, Action::Down)) && successor_end >= next_start_s - 1){

               							up_start_t = next_start_s - 1;

               							flag_re_up = true;
               						}
               					 }
               					if(isTemporalObstacleAfterT(State(s.state.x, s.state.y -2), m_lastGScore + current_cost + 1)){
               						flag_re_up = true;
               					}
               					if(isTemporalObstacleAfterT(State(s.state.x, s.state.y -2), m_lastGScore + current_cost + 1)
                						|| isTemporalObstacle(State(s.state.x - 1, s.state.y -1), m_lastGScore + current_cost + 1)
             							|| isTemporalObstacle(State(s.state.x + 1, s.state.y - 1), m_lastGScore + current_cost + 1)
         							    ||(m_env.is_limit && current_cost > m_env.limit_jump)){
                					current_successor.flag_wait = false;
                					if(up_start_t == m_lastGScore + current_cost + 1){
                						current_successor.dir = 0x0f;
                					} else current_successor.dir = 0x0b;

                					current_successor.action = Action::Down;
                					jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor, Action::Down, current_cost + 1));
                				} else {
                					if(flag_re_up){
             							if(up_start_t == m_lastGScore + current_cost + 1){
             								current_successor.dir = 0x0f;
             							} else {
             								current_successor.dir = 0x0b;
             							}
             							if(isSolution(current_successor)) flag_solution = true;
             							current_successor.flag_wait = false;
                 						jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(current_successor, Action::Up, current_cost + 1));
                					}else {
                						current_successor.dir = 0x0b;
                						getJPSSuccessors(current_successor, 0x0b, current_cost + 1, depth + 1);
                					}
                				}
            				}
             		 	 }
             	}
             	if(re_start_s.size() > 0){
             		std::sort(re_start_s.begin(), re_start_s.end());
  					Cost re_ac = -1;
  					JPSSIPPState temp_state = s;
  					temp_state.dir = 0x00;
  					for(int re_i = 0; re_i < re_start_s.size(); re_i++){
  						if(re_start_s[re_i].rt == -1) continue;
  						temp_state.dir |= re_start_s[re_i].dir;
  						temp_state.flag_wait = false;
  						for(int re_ii = re_i  + 1; re_ii < re_start_s.size(); re_ii++){
  							if(re_start_s[re_ii].rt == re_start_s[re_i].rt){
  								temp_state.dir |= re_start_s[re_ii].dir;
  							}else break;
  						}
  						if(isSolution(temp_state)) flag_solution = true;
  						temp_state.flag_wait = false;
       					jps_successors.emplace_back(Neighbor<JPSSIPPState, Action, Cost>(temp_state, Action::Left, re_start_s[re_i].rt - m_lastGScore));
  						break;
  					}
             	}
       }

    void onExpandNode(const JPSSIPPState& s, Cost fScore, Cost gScore) {
      // const auto& interval =
      // safeIntervals(m_env.getLocation(s.state)).at(s.interval);
      // std::cout << "expand: " << s.state << "," << interval.start << " to "
      // << interval.end << "(g: " << gScore << " f: " << fScore << ")" <<
      // std::endl;
      // This is called before getNeighbors(). We use the callback to find the
      // current cost (=time) of the expanded node
    	m_env.num_expansion++;
    	m_lastGScore = gScore;
        m_env.onExpandNode(s.state, fScore, gScore);
    }

    void onDiscover(const JPSSIPPState& s, Cost fScore, Cost gScore) {
      // const auto& interval =
      // safeIntervals(m_env.getLocation(s.state)).at(s.interval);
      // std::cout << "discover: " << s.state << "," << interval.start << " to "
      // << interval.end << std::endl;
      m_env.onDiscover(s.state, fScore, gScore);
    }

    void setCollisionIntervals(const Location& location,
                               const std::vector<interval>& intervals) {
      m_safeIntervals.erase(location);
      std::vector<interval> sortedIntervals(intervals);
      std::sort(sortedIntervals.begin(), sortedIntervals.end());

//      std::cout << location << ": " << std::endl;
      if (intervals.size() > 0) {
        m_safeIntervals[location]; // create empty safe interval
        int start = 0;
        int lastEnd = 0;
        for (const auto& interval : sortedIntervals) {
 //           std::cout << "  ci: " << start << " " << lastEnd << " " << interval.start << " - " << interval.end << std::endl;
          // std::endl;
          assert(interval.start <= interval.end);
          assert(start <= interval.start);
          // if (start + 1 != interval.start - 1) {
          // std::cout << start << "," << interval.start << std::endl;
          // assert(start + 1 < interval.start - 1);
          if (start <= interval.start - 1) {
            m_safeIntervals[location].push_back({start, interval.start - 1});
          }
          // }
          start = interval.end + 1;
          lastEnd = interval.end;
        }
        if (lastEnd < std::numeric_limits<int>::max()) {
          // assert(start < std::numeric_limits<int>::max());
          m_safeIntervals[location].push_back(
              {start, std::numeric_limits<int>::max()});
        }
      }
      // auto iter = m_safeIntervals.find(location);
      // if (iter != m_safeIntervals.end()) {
      //   for (const auto& si : iter->second) {
      //     std::cout << "  si: " << si.start << " - " << si.end << std::endl;
      //   }
      // }
    }

    void setEdgeCollisions(const Location& location,
  		  	  	  	  	const std::vector<edgeCollision>& edge_collision) {
  	  m_edgeCollision.erase(location);
  	  if(edge_collision.size() > 0){
  		  m_edgeCollision[location];
  		  for(const auto& ec : edge_collision){
  			  m_edgeCollision[location].push_back(ec);
  		  }
  	  }
    }

    bool IsEdgeCollisions(const Location& location, const edgeCollision& ec){
//    		std::cout << location.x << " " << location.y << " " << ec.t << " " << ec.action << std::endl;
//    		return false;
    		const auto iter = m_edgeCollision.find(location);
    		if(iter == m_edgeCollision.end()) return false;
    		if((iter->second).size() == 0) return false;
    		for (auto& cec : (iter->second)){
    			if( cec == ec ){
    				return true;
    			}
    		}
    		return false;
    }

    bool findSafeInterval(const State& state, Cost time, size_t& interval)
    {
      const auto& si = safeIntervals(m_env.getLocation(state));
      for (size_t idx = 0; idx < si.size(); ++idx) {
        if (si[idx].start <= time && si[idx].end >= time) {
          interval = idx;
          return true;
        }
      }
      return false;
    }

    bool findSafeInterval(const State& state, Cost time, size_t& interval, Cost &start_t, Cost &end_t)
    {
      const auto& si = safeIntervals(m_env.getLocation(state));
      for (size_t idx = 0; idx < si.size(); ++idx) {
        if (si[idx].start <= time && si[idx].end >= time) {
          interval = idx;
          start_t = si[idx].start;
          end_t = si[idx].end;
          return true;
        }
      }
      return false;
    }

    bool findSafeInterval(const State& state, Cost time, size_t& interval, Cost &start_t, Cost &end_t, Cost &next_start, Cost &next_end)
    {
      const auto& si = safeIntervals(m_env.getLocation(state));
      start_t = -1;
      end_t = -1;
      next_start = -1;
      next_end = -1;
//      std::cout << "Safe Inteval ---------- " << state.x << " " << state.y << "\n";
      bool first_safe = true;
      for (size_t idx = 0; idx < si.size(); ++idx) {
//          std::cout << start_t << " " << end_t << " " << next_start <<" " << next_end << "-------------------\n";
//            std::cout << "Time " << time << " " << si[idx].start << " " << si[idx].end << " "<< "-------------------\n";
          if(first_safe && si[idx].end >= time){
        	  first_safe = false;
        	  next_start = si[idx].start;
        	  next_end = si[idx].end;
          }
    	  if (si[idx].start <= time && si[idx].end >= time) {
    		  first_safe = false;
    		  interval = idx;
    		  start_t = si[idx].start;
    		  end_t = si[idx].end;
    		  if(idx + 1 < si.size()){
    			  next_start = si[idx + 1].start;
    			  next_end = si[idx + 1].end;
    		  } else{
    			  next_start = -1;
    			  next_end = -1;
    		  }
//    		  std::cout << start_t << " " << end_t << " " << next_start <<" " << next_end << "-------------------\n";
    		  return true;
        }
      }
      return false;
    }

    bool isTempolObstacle(const Location& location){
    	return m_safeIntervals.find(location) != m_safeIntervals.end();
    }

    bool isTemporalObstacle(const Location& location, Cost now_cost){

        const auto& si = safeIntervals(m_env.getLocation(location));
        if(si.size() == 0) return true;
        if(si.back().start <= now_cost && si.back().end == std::numeric_limits<Cost>::max()) return false;
        else return true;
    }

    bool isTemporalObstacleAtT(const Location& location, Cost time){
        const auto& si = safeIntervals(m_env.getLocation(location));
        if(si.size() == 0) return true;

        for (size_t idx = 0; idx < si.size(); ++idx) {
          if (si[idx].start <= time && si[idx].end >= time) {
        	  return false;
          }
        }
        return true;

    }

    bool isTemporalObstacleAfterT(const Location& location, Cost time){ // whether the obstacle appears after the time
        const auto& si = safeIntervals(m_env.getLocation(location));
//        std::cout << location.x << "  " << location.y << " Cost: " << time << " si.size " << si.size() << "\n";
        if(si.size() == 0) return true;
        Cost start_ = 0, end_ = si[0].start - 1;
        for (size_t idx = 0; idx < si.size() - 1; ++idx) {
//        	std::cout << start_ << " " << end_<<"\n";
        	if(start_ <= end_ && end_ >= time){ return true;}
        	start_ = si[idx].end + 1;
        	end_ = si[idx + 1].start - 1;
        }
    	if(start_ <= end_ && end_ >= time) { return true;}

        if(si.back().end == std::numeric_limits<Cost>::max()) { return false;}
        else { return true;}
    }

    bool isTemporalObstacleAfterT(const Location& location, Cost time, Cost &start_time){ // whether the obstacle appears after the time
        const auto& si = safeIntervals(m_env.getLocation(location));
//        std::cout << location.x << "  " << location.y << " Cost: " << time << " si.size " << si.size() << "\n";
        start_time = -1;
        if(si.size() == 0) return true;
        Cost start_ = 0, end_ = si[0].start - 1;
        for (size_t idx = 0; idx < si.size() - 1; ++idx) {
//        	std::cout << start_ << " " << end_<<"\n";
        	if(start_ <= end_ && end_ >= time){ start_time = end_; return true;}
        	start_ = si[idx].end + 1;
        	end_ = si[idx + 1].start - 1;
        }
    	if(start_ <= end_ && end_ >= time) {start_time = end_; return true;}

        if(si.back().end == std::numeric_limits<Cost>::max()) { return false;}
        else { start_time = -1; return true;}
    }

    bool isSafeAtT(const Location& location, Cost &start_time){ // whether the obstacle appears after the time
        const auto& si = safeIntervals(m_env.getLocation(location));
//       std::cout << location.x << "  " << location.y << " Cost: " << start_time << " si.size " << si.size() << "\n";

       if(si.size() == 0) return false;
        Cost start_ = 0, end_ = si[0].start - 1;
        for (size_t idx = 0; idx < si.size(); ++idx) {
        	if(si[idx].start == start_time) return true;
        }
        return false;
    }

  public:
	bool UpdateSafeInterval(Location location, Cost start_, Cost end_){
    	assert(start_<=end_);
    	auto si = m_safeIntervals.find(location);
    	assert(si != m_safeIntervals.end());

    	for(size_t idx = 0; idx < m_safeIntervals[location].size(); idx++){
    		if(si[idx].end > start_ || si[idx].start >end_) continue;
    		if(si[idx].start>= start_ && si[idx].end <= end_ ){
    			m_safeIntervals[location].erase(si + idx);
    			idx--;
    		}else if(si[idx].start >= start_ && si[idx].start <= end_ ){
    			si[idx].start = end_ + 1;
    		}else if(start_ <= si[idx].end && end_ <= si[idx]){
    			si[idx].end = start_ - 1;
    		}
    	}

    	return true;
    }
   private:
    const std::vector<interval>& safeIntervals(const Location& location) {
      static std::vector<interval> defaultInterval(
          1, {0, std::numeric_limits<Cost>::max()});
      const auto iter = m_safeIntervals.find(location);
      if (iter == m_safeIntervals.end()) {
        return defaultInterval;
      }
      return iter->second;
    }

   private:
    Environment& m_env;
    Cost m_lastGScore;
    bool flag_solution = false;
    std::unordered_map<Location, std::vector<interval> > m_safeIntervals;
    std::unordered_map<Location, std::vector<edgeCollision>> m_edgeCollision;
    std::vector<Neighbor<JPSSIPPState, Action, Cost> > jps_successors;
  };

 private:
  JPSSIPPEnvironment m_env;
  JPSAStar<JPSSIPPState, JPSSIPPAction, Cost, JPSSIPPEnvironment, JPSSIPPStateHasher, JPSSIPPStateHasher> m_astar;
};

}  // namespace libMultiRobotPlanning